Commit 7c27331c authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add control state to stoeber block and its simulators.

Add external enable output to mech html page
parent 57deb7c1
......@@ -52,6 +52,12 @@
<div style="border: thin solid black; background: lightgrey"
id="Azimuth_Enabled">
Azimuth Drive<br>
<ul>
<div style="border: thin solid black; background: lightgrey"
id="dt_az.external_enable">
External_enable<br>
</div>
</ul>
</div>
</td>
......@@ -183,6 +189,16 @@
<div style="border: thin solid black; background: lightgrey"
id="Elevation_Enabled">
Elevation Drives<br>
<ul>
<div style="border: thin solid black; background: lightgrey"
id="dt_el_r.external_enable">
External_enable Right<br>
</div>
<div style="border: thin solid black; background: lightgrey"
id="dt_el_l.external_enable">
External_enable Left<br>
</div>
</ul>
</div>
</td>
......@@ -538,6 +554,9 @@ for (i = 0; i < traceval_list.length; i++) {
Azimuth_Enabled_trace = new trace();
trace_bool('Azimuth_Enabled', Azimuth_Enabled_trace);
Azimuth_External_Enable_trace = new trace();
trace_bool('dt_az.external_enable', Azimuth_Enable_trace);
Azimuth_Drive_Safety_p270_trace = new trace();
trace_bool('Azimuth_Drive_Safety_p270', Azimuth_Drive_Safety_p270_trace);
......@@ -565,6 +584,11 @@ trace_bool('Elevation_Safe', Elevation_Safe_trace);
Elevation_Enabled_trace = new trace();
trace_bool('Elevation_Enabled', Elevation_Enabled_trace);
Elevation_External_Enable_Right_trace = new trace();
trace_bool('dt_el_r.external_enable', Elevation_External_Enable_Right_trace);
Elevation_External_Enable_trace = new trace();
trace_bool('dt_el_l.external_enable', Elevation_External_Enable_Left_trace);
......
......@@ -45,6 +45,7 @@ static struct ghost_term ghost_outputs[] = {
{ "be4", "dt_az_safety_p", "value" },
{ "be5", "dt_az_bex", "value" },
{ "ae1", "dt_az_focusbox", "value" },
{ "external_enable", "dt_az_bex", "value" },
};
static struct ghost_term ghost_inputs[] = {
......
......@@ -47,6 +47,7 @@ static struct ghost_term ghost_outputs[] = {
{ "be4", "dt_el_safety_t", "value" },
{ "be5", "dt_el_bex", "value" },
{ "ae1", "dt_el_ae1", "value" },
{ "external_enable", "dt_el_bex", "value" },
};
static struct ghost_term ghost_inputs[] = {
......
......@@ -46,6 +46,7 @@ static struct ghost_term ghost_outputs[] = {
{ "be4", "dt_el_bex", "value" },
{ "be5", "dt_el_bex", "value" },
{ "ae1", "dt_el_ae1", "value" },
{ "external_enable", "dt_el_bex", "value" },
{ NULL },
};
......
......@@ -37,27 +37,65 @@
#define STOEBER_PDO_MAP(g,nr,s,l) \
htole32((STOEBER_PARAM2INDEX(g,nr)<<16)|((s)<<8)|l)
enum stoeber_e48 {
STOEBER_E48_SELFTEST = 0,
STOEBER_E48_SWITCHON_DISABLE = 1,
STOEBER_E48_READY_SWITCHON = 2,
STOEBER_E48_SWTICHED_ON = 3,
STOEBER_E48_ENABLED = 4,
STOEBER_E48_FAULT = 5,
STOEBER_E48_FAULT_REACTION = 6,
STOEBER_E48_QUICKSTOP = 7,
};
static char *stoeber_e48_tostring(enum stoeber_e48 e)
{
switch(e) {
case STOEBER_E48_SELFTEST:
return "Self-test";
case STOEBER_E48_SWITCHON_DISABLE:
return "Switch-on disable";
case STOEBER_E48_READY_SWITCHON:
return "Ready for switch-on";
case STOEBER_E48_SWTICHED_ON:
return "Switched on";
case STOEBER_E48_ENABLED:
return "Enabled";
case STOEBER_E48_FAULT:
return "Fault";
case STOEBER_E48_FAULT_REACTION:
return "Fault reaction";
case STOEBER_E48_QUICKSTOP:
return "Quick stop";
}
}
#define RPM2RADS(val) ((val)*2.0*M_PI/60.0)
#define static_assert(e) \
do { \
enum { assert_static__ = 1/(e) }; \
} while (0)
/* rx pdo */
struct rx_pdo {
uint32_t I80;
uint32_t E91;
uint16_t E90;
uint16_t E19;
uint16_t E10;
uint32_t I80; /* current position */
uint32_t E91; /* motor speed */
uint16_t E90; /* motor torque */
uint16_t E19; /* binary inputs */
uint16_t E10; /* AE1 level */
uint8_t E48; /* device control state */
} __packed;
/* tx pdo */
struct tx_pdo {
uint8_t A180;
uint8_t F210;
uint16_t I210;
uint32_t I213;
uint16_t I215;
uint16_t C230;
uint8_t A180; /* device control byte */
uint8_t F210; /* BA control bits */
uint16_t I210; /* posi.control word */
uint32_t I213; /* target position */
uint16_t I215; /* speed */
uint16_t C230; /* torque limit */
} __packed;
struct controller_block_private {
......@@ -71,6 +109,7 @@ struct controller_block_private {
uint32_t serial_number;
bool enabled;
enum stoeber_e48 control_state;
float output_position;
float output_speed;
......@@ -80,6 +119,7 @@ struct controller_block_private {
bool output_be3;
bool output_be4;
bool output_be5;
bool output_external_enable;
float output_ae1;
float *input_speed;
......@@ -98,21 +138,12 @@ struct controller_block_private {
static void calculate_tx(struct controller_block *tx)
{
struct controller_block_private *private = tx->private;
bool enable;
bool enabled = private->enabled;
bool enabled;
float sp;
float torque;
struct timespec timeout = { 1, 0 };
enable = *private->input_enable;
if (enable && !enabled) {
esc_al_state_set(private->dev, ESC_AL_STATE_OPERATIONAL, &timeout);
enabled = true;
}
if (!enable && enabled) {
enabled = false;
}
enabled = *private->input_enable;
enabled &= private->output_external_enable;
private->tx_pdo->A180 = enabled;
private->tx_pdo->F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
......@@ -133,40 +164,89 @@ static void calculate_tx(struct controller_block *tx)
torque = *private->input_torque / private->standstill_torque * 16384;
private->tx_pdo->C230 = htole16((int16_t)torque);
private->enabled = enabled;
}
static void calculate_rx(struct controller_block *rx)
{
struct controller_block_private *private = rx->private;
struct rx_pdo *rx_pdo;
int ret;
float speed, torque, position, ae1;
bool enabled, be1, be2, be3, be4, be5;
bool external_enable;
uint16_t E19;
enum stoeber_e48 e48;
enabled = private->enabled;
if (ret == 0) {
position = (int32_t)le32toh(private->rx_pdo->I80);
rx_pdo = private->rx_pdo;
position = (int32_t)le32toh(rx_pdo->I80);
position *= 2.0 * M_PI / 16384;
speed = (int32_t)le32toh(private->rx_pdo->E91);
speed = (int32_t)le32toh(rx_pdo->E91);
speed *= 1.0/16384;
speed = RPM2RADS(speed);
torque = (int16_t)le16toh(private->rx_pdo->E90);
torque = (int16_t)le16toh(rx_pdo->E90);
torque *= 1.0 / 1200;
ae1 = (int16_t)le16toh(private->rx_pdo->E10);
ae1 = (int16_t)le16toh(rx_pdo->E10);
ae1 *= 20.0 / 32767.0;
E19 = le16toh(private->rx_pdo->E19);
E19 = le16toh(rx_pdo->E19);
be1 = E19 & 0x01;
be2 = E19 & 0x02;
be3 = E19 & 0x04;
be4 = E19 & 0x08;
be5 = E19 & 0x10;
e48 = rx_pdo->E48;
if (e48 != private->control_state) {
enum log_type lt;
switch(e48) {
case STOEBER_E48_SELFTEST:
case STOEBER_E48_SWITCHON_DISABLE:
case STOEBER_E48_READY_SWITCHON:
case STOEBER_E48_SWTICHED_ON:
if (private->control_state ==
STOEBER_E48_ENABLED)
lt = LOG_T_ERROR;
else
lt = LOG_T_WARNING;
break;
case STOEBER_E48_ENABLED:
lt = LOG_T_WARNING;
break;
case STOEBER_E48_FAULT:
case STOEBER_E48_FAULT_REACTION:
case STOEBER_E48_QUICKSTOP:
default:
lt = LOG_T_ERROR;
break;
}
log_send(lt, "%s: State changed from '%s' to '%s'",
rx->name,
stoeber_e48_tostring(private->control_state),
stoeber_e48_tostring(e48));
private->control_state = e48;
}
external_enable = e48 & STOEBER_E48_ENABLED;
if (external_enable != private->output_external_enable) {
log_send(LOG_T_WARNING,
"%s: external enable changed state to %s",
rx->name, external_enable ? "true" : "false");
}
if (!external_enable & enabled) {
log_send(LOG_T_ERROR,
"%s: external_enable prevents device from keeping enabled state",
rx->name);
enabled = false;
}
} else {
enabled = false;
speed = 0;
......@@ -178,6 +258,7 @@ static void calculate_rx(struct controller_block *rx)
be4 = false;
be5 = false;
ae1 = 0;
external_enable = false;
}
private->output_position = position;
......@@ -190,6 +271,7 @@ static void calculate_rx(struct controller_block *rx)
private->output_be4 = be4;
private->output_be5 = be5;
private->output_ae1 = ae1;
private->output_external_enable = external_enable;
}
static void callback_tx(struct controller_block *block, void *data)
......@@ -223,6 +305,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "be4", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, output_be4) },
{ "be5", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, output_be5) },
{ "ae1", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output_ae1) },
{ "external_enable", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, output_external_enable) },
{ NULL },
};
......@@ -299,6 +382,9 @@ static int pre_operational_hook(struct esc_device *esc)
if (!block->private->candev) {
block->private->candev = esc_coe_create(esc->mailbox);
block->private->candev->logname = block->name;
canopen_emergency_handler_attach(block->private->candev, stoeber_emergency_handler);
}
canopen_dev = block->private->candev;
......@@ -468,7 +554,8 @@ static int pre_operational_hook(struct esc_device *esc)
leval32 = STOEBER_PDO_MAP('E', 10, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x4, &leval32, 4);
leval32 = 0;
/* E48 (1byte) */
leval32 = STOEBER_PDO_MAP('E', 48, 0, 8);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x5, &leval32, 4);
/* Receive pdo: */
......@@ -497,6 +584,9 @@ static int pre_operational_hook(struct esc_device *esc)
leval32 = STOEBER_PDO_MAP('C', 230, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4);
/* 'wishfull thinking' -> the sample function will check against this later */
block->private->control_state = STOEBER_E48_ENABLED;
return 0;
}
......@@ -580,15 +670,17 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
esc->tx_pdo = true;
esc->tx_pdo_sm = 2;
esc->sm[2].start = 0x1600;
esc->sm[2].len = 0x000c;
esc->sm[2].len = sizeof(struct tx_pdo);
esc->sm[2].enabled = true;
static_assert(0xc == sizeof(struct tx_pdo));
/* sm3 rx pdo (slave->master) @0x1a00 */
esc->rx_pdo = true;
esc->rx_pdo_sm = 3;
esc->sm[3].start = 0x1a00;
esc->sm[3].len = 0x000e;
esc->sm[3].len = sizeof(struct rx_pdo);
esc->sm[3].enabled = true;
static_assert(0xf == sizeof(struct rx_pdo));
/* mailbox read (slave->master) @0x18f6 */
esc->sm[1].start = 0x18f6;
......@@ -610,8 +702,6 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
esc_device_initialize_operational(esc);
private->candev->logname = stoeber->name;
log_send(LOG_T_DEBUG, "Drive serial number: %d",
private->serial_number);
if (serial_number && serial_number != private->serial_number) {
......
......@@ -355,6 +355,8 @@ int esc_device_initialize_pre_operational(struct esc_device *dev)
dev->auto_recover = true;
dev->state = ESC_AL_STATE_PRE_OPERATIONAL;
return r;
}
......@@ -379,6 +381,7 @@ int esc_device_initialize_operational(struct esc_device *dev)
"%s: Could not go to state safe operational", dev->name);
goto err;
}
dev->state = ESC_AL_STATE_SAFE_OPERATIONAL;
r = esc_al_state_set(dev, ESC_AL_STATE_OPERATIONAL, &timeout);
if (r < 0) {
......@@ -386,6 +389,7 @@ int esc_device_initialize_operational(struct esc_device *dev)
"%s: Could not go to state operational", dev->name);
goto err;
}
dev->state = ESC_AL_STATE_OPERATIONAL;
err:
return r;
......
Supports Markdown
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