Commit d1365c48 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add binary outputs to drives, needed for focusbox

parent cb78daa0
......@@ -9,8 +9,11 @@ VPATH= /lib64 /usr/lib64 /usr/local/lib64 /lib /usr/lib /usr/local/lib
LIBTOOL=libtool
%.o : %.c
@echo " CC $<"
@$(CC) -MD $(CFLAGS) -c $<
@echo "LT CC $<"
@${LIBTOOL} --quiet --mode=compile --tag=CC $(CC) -MD $(CFLAGS) -c $<
# @echo " CC $<"
# @$(CC) -MD $(CFLAGS) -c $<
%: %.o
@echo " LD $@"
......
......@@ -15,7 +15,7 @@ libdt_azimuth.la_install: libdt_azimuth.la
clean:
rm -rf *.o *.a *.d *.lo *.la libs
rm -rf *.o *.a *.d *.lo *.la .libs
SRCS=$(ARCHSRCS)
......
......@@ -25,15 +25,8 @@
#include "ec_stoeber.h"
struct controller_block_private {
bool enabled;
/* bool output_be1;
bool output_be2;
bool output_be3;
bool output_be4;
bool output_be5;
float output_ae1;
*/
bool *ba1;
bool *ba2;
};
struct ghost_term {
......@@ -112,8 +105,8 @@ struct controller_block *block_dt_az_stoeber_sim_create(char *name, va_list ap)
}
for (i = 0; ghost_inputs[i].name; i++);
stoeber->inputs = i;
stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
stoeber->inputs = i + 2;
stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
if (!stoeber->input)
goto err_input;
for (i = 0; ghost_inputs[i].name; i++) {
......@@ -135,6 +128,15 @@ struct controller_block *block_dt_az_stoeber_sim_create(char *name, va_list ap)
}
}
}
stoeber->input[i].name = "ba1";
stoeber->input[i].value.b = &stoeber->private->ba1;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
i++;
stoeber->input[i].name = "ba2";
stoeber->input[i].value.b = &stoeber->private->ba2;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
stoeber->params = 0;
......
......@@ -42,6 +42,8 @@ blocks {
{ "value", "elevation_position_offset_l" }
{ "add", "elevation_position_offset_r_sum" }
{ "add", "elevation_position_offset_l_sum" }
{ "value_bool", "false" }
}
links {
......@@ -77,6 +79,8 @@ links {
{ "azimuth_speed_servo", "out", "dt_az", "speed" , true }
{ "azimuth_safety", "torque_out", "dt_az", "torque" , true }
{ "azimuth_servo_state", "enable", "dt_az", "enable" , true }
{ "false", "value", "dt_az", "ba1" , true }
{ "false", "value", "dt_az", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ "elevation_input_matrix", "out0", "elevation_spg", "cur0" , true }
......@@ -120,13 +124,16 @@ links {
{ "dt_el_l", "be1", "elevation_safety", "safety_in_l" , true }
{ "dt_el_r", "be1", "elevation_safety", "safety_in_r" , true }
{ "elevation_servo_state", "enable", "elevation_safety", "enable" , true }
{ "elevation_safety", "speed_out_r", "dt_el_r", "speed" , true }
{ "elevation_safety", "speed_out_l", "dt_el_l", "speed" , true }
{ "elevation_safety", "torque_out_r","dt_el_r", "torque" , true }
{ "elevation_safety", "torque_out_l","dt_el_l", "torque" , true }
{ "elevation_safety", "speed_out_r", "dt_el_r", "speed" , true }
{ "elevation_safety", "speed_out_l", "dt_el_l", "speed" , true }
{ "elevation_safety", "torque_out_r","dt_el_r", "torque" , true }
{ "elevation_safety", "torque_out_l","dt_el_l", "torque" , true }
{ "elevation_servo_state", "enable", "dt_el_r", "enable" , true }
{ "elevation_servo_state", "enable", "dt_el_l", "enable" , true }
{ "false", "value", "dt_el_r", "ba1" , true }
{ "false", "value", "dt_el_r", "ba2" , true }
{ "false", "value", "dt_el_l", "ba1" , true }
{ "false", "value", "dt_el_l", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_torsion_spg", "reset" , false }
{ "elevation_input_matrix", "out1", "elevation_torsion_spg", "cur0" , true }
......
......@@ -19,6 +19,6 @@ dt_elevation: dt_elevation.a($(OBJS))
clean:
rm -rf *.o *.a *.d
rm -rf *.o *.a *.d *.lo .libs *la
include ../build.mk
......@@ -25,6 +25,11 @@
#include "esc.h"
#include "ec_stoeber.h"
struct controller_block_private {
bool *ba1;
bool *ba2;
};
struct ghost_term {
char *name;
char *realblock;
......@@ -54,6 +59,7 @@ static struct ghost_term ghost_inputs[] = {
struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap)
{
struct controller_block *stoeber;
struct controller_block_private *private;
int i;
stoeber = malloc(sizeof(struct controller_block));
......@@ -66,7 +72,11 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
if (!stoeber->name)
goto err_name;
stoeber->private = NULL;
private = calloc(1, sizeof(struct controller_block_private));
if (!private)
goto err_private;
stoeber->private = private;
for (i = 0; ghost_outputs[i].name; i++);
stoeber->outputs = i;
......@@ -98,8 +108,8 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
}
for (i = 0; ghost_inputs[i].name; i++);
stoeber->inputs = i;
stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
stoeber->inputs = i + 2;
stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
if (!stoeber->input)
goto err_input;
for (i = 0; ghost_inputs[i].name; i++) {
......@@ -121,6 +131,15 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
}
}
}
stoeber->input[i].name = "ba1";
stoeber->input[i].value.b = &stoeber->private->ba1;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
i++;
stoeber->input[i].name = "ba2";
stoeber->input[i].value.b = &stoeber->private->ba2;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
stoeber->params = 0;
......@@ -134,6 +153,8 @@ err_input:
err_ghost_output:
free(stoeber->output);
err_output:
free(stoeber->private);
err_private:
free(stoeber->name);
err_name:
free(stoeber);
......
......@@ -25,6 +25,11 @@
#include "esc.h"
#include "ec_stoeber.h"
struct controller_block_private {
bool *ba1;
bool *ba2;
};
struct ghost_term {
char *name;
char *realblock;
......@@ -55,6 +60,7 @@ static struct ghost_term ghost_inputs[] = {
struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap)
{
struct controller_block *stoeber;
struct controller_block_private *private;
int i;
stoeber = malloc(sizeof(struct controller_block));
......@@ -67,7 +73,10 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
if (!stoeber->name)
goto err_name;
stoeber->private = NULL;
private = calloc(1, sizeof(struct controller_block_private));
if (!private)
goto err_private;
stoeber->private = private;
for (i = 0; ghost_outputs[i].name; i++);
stoeber->outputs = i;
......@@ -99,8 +108,8 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
}
for (i = 0; ghost_inputs[i].name; i++);
stoeber->inputs = i;
stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
stoeber->inputs = i + 2;
stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
if (!stoeber->input)
goto err_input;
for (i = 0; ghost_inputs[i].name; i++) {
......@@ -126,6 +135,16 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
ghost_inputs[i].realblock, ghost_inputs[i].realname);
}
}
stoeber->input[i].name = "ba1";
stoeber->input[i].value.b = &stoeber->private->ba1;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
i++;
stoeber->input[i].name = "ba2";
stoeber->input[i].value.b = &stoeber->private->ba2;
stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber->input[i].ghostof = NULL;
stoeber->params = 0;
stoeber->param_get = NULL;
......@@ -141,6 +160,8 @@ err_input:
err_ghost_output:
free(stoeber->output);
err_output:
free(stoeber->private);
err_private:
free(stoeber->name);
err_name:
free(stoeber);
......
......@@ -44,6 +44,8 @@ struct controller_block_private {
float *input_speed;
float *input_torque;
bool *input_enable;
bool *input_ba1;
bool *input_ba2;
};
static void calculate_tx(struct controller_block *tx)
......@@ -64,7 +66,7 @@ static void calculate_tx(struct controller_block *tx)
}
*stbr->tx_A180 = enabled;
*stbr->tx_F210 = 0;
*stbr->tx_F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
*stbr->tx_I210 = htole16(le16toh(*stbr->tx_I210) ^ 1);
if (enabled)
......@@ -330,7 +332,7 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
stoeber_tx->outputs = 0;
stoeber_tx->output = NULL;
stoeber_tx->inputs = 3;
stoeber_tx->inputs = 5;
stoeber_tx->input = calloc(stoeber_tx->inputs, sizeof(struct controller_block_interm));
if (!stoeber_tx->input)
goto err_tx_input;
......@@ -346,6 +348,14 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
stoeber_tx->input[2].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_tx->input[2].value.b = &private->input_enable;
stoeber_tx->input[2].ghostof = NULL;
stoeber_tx->input[3].name = "ba1";
stoeber_tx->input[3].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_tx->input[3].value.b = &private->input_ba1;
stoeber_tx->input[3].ghostof = NULL;
stoeber_tx->input[4].name = "ba2";
stoeber_tx->input[4].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_tx->input[4].value.b = &private->input_ba2;
stoeber_tx->input[4].ghostof = NULL;
stoeber->inputs = stoeber_tx->inputs;
stoeber->input = calloc(stoeber->inputs, sizeof(struct controller_block_interm));
......
......@@ -87,126 +87,126 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
/* A180 device control, disable it for now */
val8 = 0x00;
canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
/* A61 fault reset source to parameter */
val8 = 2;
canopen_write_param(canopen_dev, 0x203d, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 61), 0x0, &val8, 1);
usleep(3000);
/* A180 device control, reset faults */
val8 = 0x02;
canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
usleep(50000);
/* A180 device control, reset faults */
val8 = 0x00;
canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
/* A21 brake resistor */
leval16 = htole16(stbr->brake_resistor * 10.0);
canopen_write_param(canopen_dev, 0x2015, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 21), 0x0, &leval16, 2);
/* A22 brake resistor power */
leval16 = htole16(stbr->brake_power);
canopen_write_param(canopen_dev, 0x2016, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 22), 0x0, &leval16, 2);
/* A29 fault quickstop */
val8 = 1;
canopen_write_param(canopen_dev, 0x201d, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 29), 0x0, &val8, 1);
/* A34 auto start */
val8 = 1;
canopen_write_param(canopen_dev, 0x2022, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 34), 0x0, &val8, 1);
/* A45*/
val8 = 1;
canopen_write_param(canopen_dev, 0x202d, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 45), 0x0, &val8, 1);
/* A60*/
val8 = 1;
canopen_write_param(canopen_dev, 0x203c, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 60), 0x0, &val8, 1);
/* A150 cycle time */
val8 = 0x06;
canopen_write_param(canopen_dev, 0x2096, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 150), 0x0, &val8, 1);
/* A258 set PDO timeout */
leval16 = htole16(1000);
canopen_write_param(canopen_dev, 0x2102, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 258), 0x0, &leval16, 2);
/* G90 activate PLL */
val8 = 1;
canopen_write_param(canopen_dev, 0x2c5a, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 90), 0x0, &val8, 1);
/* G91 pll phase offset */
leval32 = htole32(0);
canopen_write_param(canopen_dev, 0x2c5b, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 91), 0x0, &leval32, 4);
/* G92 pll gain pk */
leval32 = htole32(10);
canopen_write_param(canopen_dev, 0x2c5c, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 92), 0x0, &leval32, 4);
/* G93 pll low pass */
leval32 = htole32(999);
canopen_write_param(canopen_dev, 0x2c5d, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 93), 0x0, &leval32, 4);
/* G98 cycle to 4ms */
leval16 = htole16(4000);
canopen_write_param(canopen_dev, 0x2c62, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 98), 0x0, &leval16, 2);
/* Set I06 to 2 decimal digits */
val8 = 2;
canopen_write_param(canopen_dev, 0x3006, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 6), 0x0, &val8, 1);
/* C32 (motor integrator) to 3000 ms */
leval16 = htole16(32000);
canopen_write_param(canopen_dev, 0x2420, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 32), 0x0, &leval16, 2);
/* C33 low pass reference speed 1 ms */
leval32 = htole32(10);
canopen_write_param(canopen_dev, 0x2421, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 33), 0x0, &leval32, 4);
/* C130 torque limit source to parameter */
val8 = 4;
canopen_write_param(canopen_dev, 0x2482, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 130), 0x0, &val8, 1);
/* Set I10 to maximum speed */
leval32 = htole32(stbr->max_speed / (2*M_PI) * 360 * 100);
// leval32 = htole32(3000 * 360 / 60 * 100);
canopen_write_param(canopen_dev, 0x300a, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 10), 0x0, &leval32, 4);
/* Set I11 to maximum acceleration */
// leval32 = htole32(2000000 * 100);
leval32 = htole32(stbr->max_accel * 100);
canopen_write_param(canopen_dev, 0x300b, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 11), 0x0, &leval32, 4);
/* Set I25 to 100% speed ff */
leval16 = htole16(100);
canopen_write_param(canopen_dev, 0x3019, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 25), 0x0, &leval16, 2);
/* Set I30 to 1 (encoder signal 0) */
val8 = 1;
canopen_write_param(canopen_dev, 0x301e, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 30), 0x0, &val8, 1);
/* Set I37 to 1 (active) */
val8 = 1;
canopen_write_param(canopen_dev, 0x3025, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 37), 0x0, &val8, 1);
/* Set I100 to parameter (execute source) */
val8 = 2;
canopen_write_param(canopen_dev, 0x3064, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 100), 0x0, &val8, 1);
/* B17 Torque M0 */
canopen_read_param(canopen_dev, 0x2211, 0x0, &leval32, 4);
canopen_read_param(canopen_dev, STOEBER_PARAM2INDEX('B', 17), 0x0, &leval32, 4);
printf("B17: %08x %d\n", le32toh(leval32), le32toh(leval32));
stbr->standstill_torque = (float)(le32toh(leval32)) / 1000.0;
printf("Standstill torque: %f\n", stbr->standstill_torque);
/* C03 max pos torque 150% of standstill */
leval32 = htole32(150);
canopen_write_param(canopen_dev, 0x2403, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 3), 0x0, &leval32, 4);
/* C05 max neg torque -150% of standstill */
leval32 = htole32(-150);
canopen_write_param(canopen_dev, 0x2405, 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 5), 0x0, &leval32, 4);
// /* A180 device control, enable it to make sure we stay
// at the same position */
......@@ -215,66 +215,66 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
/* I211 command byte to 4 (move_velocity) */
val8 = 0x04;
canopen_write_param(canopen_dev, 0x30d3, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 211), 0x0, &val8, 1);
/* I215 v to 0 */
leval16 = htole16(0);
canopen_write_param(canopen_dev, 0x30d7, 0x0, &leval16, 2);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 215), 0x0, &leval16, 2);
/* I210 latch command */
val8 = 0x00;
canopen_write_param(canopen_dev, 0x30d2, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 210), 0x0, &val8, 1);
val8 = 0x01;
canopen_write_param(canopen_dev, 0x30d2, 0x0, &val8, 1);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 210), 0x0, &val8, 1);
/* Transmit pdo: */
/* I80 (4bytes) */
leval32 = STOEBER_PDO_MAP(0x3050, 0, 32);
canopen_write_param(canopen_dev, 0x20e9, 0x0, &leval32, 4);
leval32 = STOEBER_PDO_MAP('I', 80, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x0, &leval32, 4);
/* E91 (4bytes) */
leval32 = STOEBER_PDO_MAP(0x285b, 0, 32);
canopen_write_param(canopen_dev, 0x20e9, 0x1, &leval32, 4);
leval32 = STOEBER_PDO_MAP('E', 91, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x1, &leval32, 4);
/* E90 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x285a, 0, 16);
canopen_write_param(canopen_dev, 0x20e9, 0x2, &leval32, 4);
leval32 = STOEBER_PDO_MAP('E', 90, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x2, &leval32, 4);
/* E19 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x2813, 0, 16);
canopen_write_param(canopen_dev, 0x20e9, 0x3, &leval32, 4);
leval32 = STOEBER_PDO_MAP('E', 19, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x3, &leval32, 4);
/* E10 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x280a, 0, 16);
canopen_write_param(canopen_dev, 0x20e9, 0x4, &leval32, 4);
leval32 = STOEBER_PDO_MAP('E', 10, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x4, &leval32, 4);
leval32 = 0;
canopen_write_param(canopen_dev, 0x20e9, 0x5, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x5, &leval32, 4);
/* Receive pdo: */
/* A180 (1byte) */
leval32 = STOEBER_PDO_MAP(0x20b4, 0, 32);
canopen_write_param(canopen_dev, 0x20e1, 0x0, &leval32, 4);
leval32 = STOEBER_PDO_MAP('A', 180, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x0, &leval32, 4);
/* F210 (1byte) */
leval32 = STOEBER_PDO_MAP(0x2ad2, 0, 32);
canopen_write_param(canopen_dev, 0x20e1, 0x1, &leval32, 4);
leval32 = STOEBER_PDO_MAP('F', 210, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x1, &leval32, 4);
/* I210 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x30d2, 0, 16);
canopen_write_param(canopen_dev, 0x20e1, 0x2, &leval32, 4);
leval32 = STOEBER_PDO_MAP('I', 210, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x2, &leval32, 4);
/* I213 (4bytes) */
leval32 = STOEBER_PDO_MAP(0x30d5, 0, 16);
canopen_write_param(canopen_dev, 0x20e1, 0x3, &leval32, 4);
leval32 = STOEBER_PDO_MAP('I', 213, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x3, &leval32, 4);
/* I215 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x30d7, 0, 16);
canopen_write_param(canopen_dev, 0x20e1, 0x4, &leval32, 4);
leval32 = STOEBER_PDO_MAP('I', 215, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x4, &leval32, 4);
/* C230 (2bytes) */
leval32 = STOEBER_PDO_MAP(0x24e6, 0, 16);
canopen_write_param(canopen_dev, 0x20e1, 0x5, &leval32, 4);
leval32 = STOEBER_PDO_MAP('C', 230, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4);
esc_coe_destroy(canopen_dev);
esc_mailbox_destroy(mb);
......
......@@ -23,7 +23,10 @@
#include "ec.h"
#include "canopen.h"
#define STOEBER_PDO_MAP(i,s,l) htole32(((i)<<16)|((s)<<8)|l)
#define STOEBER_PARAM2INDEX(group, nr) (0x2000 + ((group) - 'A') * 0x200 + nr)
#define STOEBER_PDO_MAP(g,nr,s,l) \
htole32((STOEBER_PARAM2INDEX(g,nr)<<16)|((s)<<8)|l)
#define STOEBER_PDO_A180 0
#define STOEBER_PDO_F210 1
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment