Commit 502d9a90 authored by Michel Roelofs's avatar Michel Roelofs
Browse files

Merge remote-tracking branch 'eris/jeroen' into michel

parents bf9a8e27 0e2c9c21
......@@ -9,6 +9,6 @@ blocks: noil.o test.o oneshot.o
clean:
rm -rf *.o *.d
rm -rf *.o *.lo *.d *.h
include ../../../build.mk
/* Automaticly generated block 'oneshot' */
#ifndef _INCLUDE_oneshot_H_
#define _INCLUDE_oneshot_H_
struct controller_block * block_oneshot_create(char *name);
#endif /* _INCLUDE_oneshot_H_ */
/* Automaticly generated block 'test' */
#ifndef _INCLUDE_test_H_
#define _INCLUDE_test_H_
struct controller_block * block_test_create(char *name);
#endif /* _INCLUDE_test_H_ */
......@@ -65,9 +65,9 @@ static void calculate_tx(struct controller_block *tx)
enabled = true;
}
*stbr->tx_A180 = enabled;
*stbr->tx_F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
*stbr->tx_I210 = htole16(le16toh(*stbr->tx_I210) ^ 1);
stbr->tx.A180 = enabled;
stbr->tx.F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
stbr->tx.I210 = htole16(le16toh(stbr->tx.I210) ^ 1);
if (enabled)
sp = *private->input_speed * 16384.0 / RPM2RADS(3000.0);
......@@ -79,12 +79,13 @@ static void calculate_tx(struct controller_block *tx)
if (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;
*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);
if (ret && enabled) {
......@@ -107,20 +108,22 @@ static void calculate_rx(struct controller_block *rx)
ret = ec_rx_pdo(controller_samplenr);
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;
speed = (int32_t)le32toh(*stbr->rx_E91);
speed = (int32_t)le32toh(stbr->rx.E91);
speed *= 1.0/16384;
speed = RPM2RADS(speed);
torque = (int16_t)le16toh(*stbr->rx_E90);
torque = (int16_t)le16toh(stbr->rx.E90);
torque *= 1.0 / 1200;
ae1 = (int16_t)le16toh(*stbr->rx_E10);
ae1 = (int16_t)le16toh(stbr->rx.E10);
ae1 *= 20.0 / 32767.0;
E19 = le16toh(*stbr->rx_E19);
E19 = le16toh(stbr->rx.E19);
be1 = E19 & 0x01;
be2 = E19 & 0x02;
be3 = E19 & 0x04;
......
......@@ -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)
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");
return 0;
......
......@@ -28,19 +28,6 @@
#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
#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 {
char *dev;
struct ec_dgram_addr addr;
......@@ -58,20 +45,24 @@ struct ec_stoeber {
/* rx pdo */
unsigned char *rx_buffer; /* 14 */
uint32_t *rx_I80;
uint32_t *rx_E91;
uint16_t *rx_E90;
uint16_t *rx_E19;
uint16_t *rx_E10;
struct {
uint32_t I80;
uint32_t E91;
uint16_t E90;
uint16_t E19;
uint16_t E10;
} rx;
/* tx pdo */
unsigned char *tx_buffer; /* 12 */;
uint8_t *tx_A180;
uint8_t *tx_F210;
uint16_t *tx_I210;
uint32_t *tx_I213;
uint16_t *tx_I215;
uint16_t *tx_C230;
struct {
uint8_t A180;
uint8_t F210;
uint16_t I210;
uint32_t I213;
uint16_t I215;
uint16_t C230;
} tx;
};
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