diff --git a/firmware/hw_layer/drivers/gpio/can_gpio.cpp b/firmware/hw_layer/drivers/gpio/can_gpio.cpp index fe4f3f18788..f9965b47c2e 100644 --- a/firmware/hw_layer/drivers/gpio/can_gpio.cpp +++ b/firmware/hw_layer/drivers/gpio/can_gpio.cpp @@ -9,6 +9,7 @@ #include "gpio/gpio_ext.h" #include "can_listener.h" +#include "can_msg_tx.h" #include /*==========================================================================*/ @@ -29,6 +30,9 @@ #define CAN_IOBOX_WHOAMI 0x08 #define CAN_IOBOX_ADC14 0x09 #define CAN_IOBOX_ADC57 0x0A +#define CAN_IOBOX_TACH1 0x0B +/* ... */ +#define CAN_IOBOX_TACH4 0x0E struct pwm_settings { beuint16_t on; @@ -37,6 +41,9 @@ struct pwm_settings { /* Base + 0x00 */ /* "Are you there?" packet with zero payload */ +struct iobox_ping { + /* empty */ +} __attribute__((packed)); /* Base + 0x01 */ struct iobox_cfg { @@ -90,6 +97,15 @@ struct iobox_adc57 { static_assert(sizeof(iobox_adc57) == 8); +/* Base + 0x0B..0x0E */ +struct iobox_tach { + beuint32_t period; + beuint16_t n_teeth; + beuint16_t total_tooth; +} __attribute__((packed)); + +static_assert(sizeof(iobox_tach) == 8); + /*==========================================================================*/ /* Driver local definitions. */ /*==========================================================================*/ @@ -99,17 +115,34 @@ static_assert(sizeof(iobox_adc57) == 8); /* ADC input count */ #define MSIOBOX_ADC_IN_COUNT 7 /* Speed in sensors */ -#define MSIOBOX_SPEED_IN_COUNT 4 +#define MSIOBOX_TACH_IN_COUNT 4 + +/* timeouts for different states */ +#define MSIOBOX_PING_TIMEOUT 100 +#define MSIOBOX_RESTART_TIMEOUT 1000 /*==========================================================================*/ /* Driver local variables and types. */ /*==========================================================================*/ +typedef enum { + MSIOBOX_DISABLED = 0, + MSIOBOX_WAIT_INIT, + MSIOBOX_WAIT_WHOAMI, + MSIOBOX_APPLY_SETTINGS, + MSIOBOX_READY, + MSIOBOX_FAILED +} msiobox_state; + class MsIoBox final : public GpioChip, public CanListener { public: - MsIoBox(uint32_t base, uint16_t period); + MsIoBox(); + MsIoBox(uint32_t bus, uint32_t base, uint16_t period); + CanListener* request() override; bool acceptFrame(const CANRxFrame& frame) const override; + + int init() override; #if 0 /* pin argument is pin number within gpio chip, not a global number */ int writePad(size_t pin, int value) override { @@ -117,7 +150,16 @@ class MsIoBox final : public GpioChip, public CanListener { return 0; } #endif + +protected: + void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override; + private: + int ping(); + int setup(); + int update(); + void checkState(); + /* Output states */ uint8_t OutMode; // on/off (0) vs pwm (1) bitfield uint8_t OutVal; // for on/off outputs @@ -132,30 +174,48 @@ class MsIoBox final : public GpioChip, public CanListener { /* Logical inputs */ uint8_t InVal; /* Wheel speed */ - // TODO + struct { + uint32_t period; + uint16_t teeths; + uint16_t totalTooth; + } Tach[MSIOBOX_TACH_IN_COUNT]; /* Can settings */ + uint32_t bus; uint32_t base; uint32_t period; + /* IOBox timebase */ + uint32_t pwmPeriodNs; + uint32_t tachinPeriodNs; /* Misc */ uint8_t version; + /* Flags */ + bool needUpdate; + bool needUpdateConfig; /* */ - uint8_t state; + msiobox_state state; + Timer stateTimer; }; +MsIoBox::MsIoBox() + : CanListener(0), bus(0), base(0), period(20) { +} -MsIoBox::MsIoBox(uint32_t base, uint16_t period) - : CanListener(0), base(base), period(period) -{ - memset(&state, 0, sizeof(state)); - +MsIoBox::MsIoBox(uint32_t bus, uint32_t base, uint16_t period) + : CanListener(0), bus(bus), base(base), period(period) { /* Need init state */ - state = 0; + state = MSIOBOX_WAIT_INIT; + stateTimer.reset(); } -bool MsIoBox::acceptFrame(const CANRxFrame& frame) const +int MsIoBox::init() { - /* 11 bit */ + /* TODO: register can listener here */ + return 0; +} + +bool MsIoBox::acceptFrame(const CANRxFrame& frame) const { + /* 11 bit only */ if (CAN_ISX(frame)) { return false; } @@ -171,9 +231,163 @@ bool MsIoBox::acceptFrame(const CANRxFrame& frame) const return false; } -void initCanGpio() { -// CAN_PIN_0 +/* Ping iobox */ +int MsIoBox::ping() { + CanTxTyped frame(CanCategory::MEGASQUIRT, base + CAN_IOBOX_PING, false, 0); + + state = MSIOBOX_WAIT_WHOAMI; + stateTimer.reset(); + return 0; +} + +/* Send init settings */ +int MsIoBox::setup() { + CanTxTyped cfg(CanCategory::MEGASQUIRT, base + CAN_IOBOX_CONFIG, false, 0); + + cfg->pwm_mask = OutMode; + cfg->tachin_mask = InMode; + cfg->adc_broadcast_interval = period; + cfg->tach_broadcast_interval = period; + + state = MSIOBOX_READY; + stateTimer.reset(); + + return 0; +} +/* Send current gpio and pwm states */ +int MsIoBox::update() { + /* TODO: protect against OutPwm/OutVal change while we are here */ + + /* PWM1 .. PWM6 */ + for (size_t i = 0; i < 3; i++) { + /* sent if PWMs in use */ + if ((OutMode & (BIT(i) | BIT(i + 1))) == 0) + continue; + + CanTxTyped pwm(CanCategory::MEGASQUIRT, base + CAN_IOBOX_SET_PWM(i), false, 0); + pwm->ch[0].on = OutPwm[i * 2].OnPeriod; + pwm->ch[0].off = OutPwm[i * 2].OffPeriod; + pwm->ch[1].on = OutPwm[i * 2 + 1].OnPeriod; + pwm->ch[1].off = OutPwm[i * 2 + 1].OffPeriod; + } + + /* PWM7 periods and on/off outputs bitfield - sent always */ + { + CanTxTyped pwm(CanCategory::MEGASQUIRT, base + CAN_IOBOX_SET_PWM(3), false, 0); + + pwm->ch[0].on = OutPwm[MSIOBOX_OUT_COUNT - 1].OnPeriod; + pwm->ch[0].off = OutPwm[MSIOBOX_OUT_COUNT - 1].OffPeriod; + pwm->out_state = OutVal; + } + + return 0; +} + +void MsIoBox::decodeFrame(const CANRxFrame& frame, efitick_t) { + uint32_t id = CAN_ID(frame); + uint32_t offset = id - base; + bool handled = true; + + if (state == MSIOBOX_READY) { + if (offset == CAN_IOBOX_ADC14) { + auto data = reinterpret_cast(&frame.data8[0]); + + for (size_t i = 0; i < 4; i++) { + AdcValue[i] = data->adc[i]; + } + } else if (offset == CAN_IOBOX_ADC57) { + auto data = reinterpret_cast(&frame.data8[0]); + + InVal = data->inputs; + for (size_t i = 0; i < 3; i++) { + AdcValue[i + 4] = data->adc[i]; + } + } else if ((offset >= CAN_IOBOX_TACH1) && (offset <= CAN_IOBOX_TACH4)) { + size_t i = offset - CAN_IOBOX_TACH1; + auto data = reinterpret_cast(&frame.data8[0]); + + /* TODO: should be atomic, add lock here? */ + Tach[i].period = data->period; + Tach[i].teeths = data->n_teeth; + Tach[i].totalTooth = data->total_tooth; + } else { + handled = false; + } + } else if (state == MSIOBOX_WAIT_WHOAMI) { + if (offset == CAN_IOBOX_WHOAMI) { + auto data = reinterpret_cast(&frame.data8[0]); + + version = data->version; + /* convert from 0.01 uS units to nS */ + pwmPeriodNs = data->pwm_period * 10; + tachinPeriodNs = data->tachin_period * 10; + + /* apply settings and set sync output states */ + setup(); + update(); + } else { + handled = false; + } + /* ignore everything else */ + } else { + handled = false; + } + + if (handled) { + /* TODO: check that we receive EVERY expected packed? */ + stateTimer.reset(); + } +} + +void MsIoBox::checkState(void) +{ + if (state == MSIOBOX_READY) { + if (needUpdateConfig) { + setup(); + needUpdateConfig = false; + /* Force update */ + needUpdate = true; + } + if (needUpdate) { + update(); + } + } else if (state == MSIOBOX_WAIT_INIT) { + ping(); + } else if (state == MSIOBOX_WAIT_WHOAMI) { + if (stateTimer.hasElapsedMs(MSIOBOX_PING_TIMEOUT)) { + state = MSIOBOX_FAILED; + stateTimer.reset(); + } + } else if (state == MSIOBOX_FAILED) { + if (stateTimer.hasElapsedMs(MSIOBOX_RESTART_TIMEOUT)) { + state = MSIOBOX_WAIT_INIT; + stateTimer.reset(); + } + } else if (state == MSIOBOX_READY) { + if (stateTimer.hasElapsedMs(period * 3)) { + state = MSIOBOX_FAILED; + stateTimer.reset(); + } + } +} + +CanListener* MsIoBox::request(void) { + checkState(); + + /* return next */ + return CanListener::request(); +} + +static MsIoBox instance[BOARD_CAN_GPIO_COUNT]; + +void initCanGpio() { + // CAN_PIN_0 + for (size_t i = 0; i < BOARD_CAN_GPIO_COUNT; i++) { + /* TODO: pick can bus and CAN ID base from settings */ + //instance[i].MsIoBox(0, CAN_IOBOX_BASE0, 20); + registerCanListener(instance[i]); + } } -#endif // EFI_CAN_GPIO \ No newline at end of file +#endif // EFI_CAN_GPIO