Commit 0e2c9c21 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Make stoeber rx and tx buffer handling alignment safe.

parent f505a66d
...@@ -65,9 +65,9 @@ static void calculate_tx(struct controller_block *tx) ...@@ -65,9 +65,9 @@ static void calculate_tx(struct controller_block *tx)
enabled = true; enabled = true;
} }
*stbr->tx_A180 = enabled; stbr->tx.A180 = enabled;
*stbr->tx_F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1); stbr->tx.F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
*stbr->tx_I210 = htole16(le16toh(*stbr->tx_I210) ^ 1); stbr->tx.I210 = htole16(le16toh(stbr->tx.I210) ^ 1);
if (enabled) if (enabled)
sp = *private->input_speed * 16384.0 / RPM2RADS(3000.0); sp = *private->input_speed * 16384.0 / RPM2RADS(3000.0);
...@@ -79,12 +79,13 @@ static void calculate_tx(struct controller_block *tx) ...@@ -79,12 +79,13 @@ static void calculate_tx(struct controller_block *tx)
if (sp < -32768) if (sp < -32768)
sp = -32768; sp = -32768;
*stbr->tx_I215 = htole16((int16_t)sp); stbr->tx.I215 = htole16((int16_t)sp);
torque = *private->input_torque / stbr->standstill_torque * 16384; torque = *private->input_torque / stbr->standstill_torque * 16384;
*stbr->tx_C230 = htole16((int16_t)torque); stbr->tx.C230 = htole16((int16_t)torque);
memcpy(stbr->tx_buffer, &stbr->tx, sizeof(stbr->tx));
ret = ec_tx_pdo(controller_samplenr); ret = ec_tx_pdo(controller_samplenr);
if (ret && enabled) { if (ret && enabled) {
...@@ -107,20 +108,22 @@ static void calculate_rx(struct controller_block *rx) ...@@ -107,20 +108,22 @@ static void calculate_rx(struct controller_block *rx)
ret = ec_rx_pdo(controller_samplenr); ret = ec_rx_pdo(controller_samplenr);
if (ret == 0) { if (ret == 0) {
position = (int32_t)le32toh(*stbr->rx_I80); memcpy(&stbr->rx, stbr->rx_buffer, sizeof(stbr->rx));
position = (int32_t)le32toh(stbr->rx.I80);
position *= 2.0 * M_PI / 16384; position *= 2.0 * M_PI / 16384;
speed = (int32_t)le32toh(*stbr->rx_E91); speed = (int32_t)le32toh(stbr->rx.E91);
speed *= 1.0/16384; speed *= 1.0/16384;
speed = RPM2RADS(speed); speed = RPM2RADS(speed);
torque = (int16_t)le16toh(*stbr->rx_E90); torque = (int16_t)le16toh(stbr->rx.E90);
torque *= 1.0 / 1200; torque *= 1.0 / 1200;
ae1 = (int16_t)le16toh(*stbr->rx_E10); ae1 = (int16_t)le16toh(stbr->rx.E10);
ae1 *= 20.0 / 32767.0; ae1 *= 20.0 / 32767.0;
E19 = le16toh(*stbr->rx_E19); E19 = le16toh(stbr->rx.E19);
be1 = E19 & 0x01; be1 = E19 & 0x01;
be2 = E19 & 0x02; be2 = E19 & 0x02;
be3 = E19 & 0x04; be3 = E19 & 0x04;
......
...@@ -376,19 +376,6 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -376,19 +376,6 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_SAFE_OPERATIONAL) < 0) if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_SAFE_OPERATIONAL) < 0)
return -1; return -1;
stbr->rx_I80 = (uint32_t*)(stbr->rx_buffer + STOEBER_PDO_I80);
stbr->rx_E91 = (uint32_t*)(stbr->rx_buffer + STOEBER_PDO_E91);
stbr->rx_E90 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E90);
stbr->rx_E19 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E19);
stbr->rx_E10 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E10);
stbr->tx_A180 = (uint8_t*)(stbr->tx_buffer + STOEBER_PDO_A180);
stbr->tx_F210 = (uint8_t*)(stbr->tx_buffer + STOEBER_PDO_F210);
stbr->tx_I210 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_I210);
stbr->tx_I213 = (uint32_t*)(stbr->tx_buffer + STOEBER_PDO_I213);
stbr->tx_I215 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_I215);
stbr->tx_C230 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_C230);
printf("Done\n"); printf("Done\n");
return 0; return 0;
......
...@@ -28,19 +28,6 @@ ...@@ -28,19 +28,6 @@
#define STOEBER_PDO_MAP(g,nr,s,l) \ #define STOEBER_PDO_MAP(g,nr,s,l) \
htole32((STOEBER_PARAM2INDEX(g,nr)<<16)|((s)<<8)|l) htole32((STOEBER_PARAM2INDEX(g,nr)<<16)|((s)<<8)|l)
#define STOEBER_PDO_A180 0
#define STOEBER_PDO_F210 1
#define STOEBER_PDO_I210 2
#define STOEBER_PDO_I213 4
#define STOEBER_PDO_I215 8
#define STOEBER_PDO_C230 10
#define STOEBER_PDO_I80 0
#define STOEBER_PDO_E91 4
#define STOEBER_PDO_E90 8
#define STOEBER_PDO_E19 10
#define STOEBER_PDO_E10 12
struct ec_stoeber { struct ec_stoeber {
char *dev; char *dev;
struct ec_dgram_addr addr; struct ec_dgram_addr addr;
...@@ -58,20 +45,24 @@ struct ec_stoeber { ...@@ -58,20 +45,24 @@ struct ec_stoeber {
/* rx pdo */ /* rx pdo */
unsigned char *rx_buffer; /* 14 */ unsigned char *rx_buffer; /* 14 */
uint32_t *rx_I80; struct {
uint32_t *rx_E91; uint32_t I80;
uint16_t *rx_E90; uint32_t E91;
uint16_t *rx_E19; uint16_t E90;
uint16_t *rx_E10; uint16_t E19;
uint16_t E10;
} rx;
/* tx pdo */ /* tx pdo */
unsigned char *tx_buffer; /* 12 */; unsigned char *tx_buffer; /* 12 */;
uint8_t *tx_A180; struct {
uint8_t *tx_F210; uint8_t A180;
uint16_t *tx_I210; uint8_t F210;
uint32_t *tx_I213; uint16_t I210;
uint16_t *tx_I215; uint32_t I213;
uint16_t *tx_C230; uint16_t I215;
uint16_t C230;
} tx;
}; };
int ec_stoeber_init(struct ec_stoeber *stbr); int ec_stoeber_init(struct ec_stoeber *stbr);
......
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