aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_IOMCU: fixed event handling bug in IOMCU code

Browse Source 
if a a message send fails to IOMCU then we were discarding all
currently pending non-recurring events. This means that states like
oneshot enable would be lost if the IOMCU reset.

We now re-trigger all events that have not yet been handled
This commit is contained in:
Andrew Tridgell 2021-02-24 08:47:24 +11:00
parent 9126579b77
commit 9d87645c6e
2 changed files with 43 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -85,11 +85,11 @@ void AP_IOMCU::init(void)
/* /*
handle event failure handle event failure
*/ */
void AP_IOMCU::event_failed(uint8_t event) void AP_IOMCU::event_failed(uint32_t event_mask)
{ {
// wait 0.5ms then retry // wait 0.5ms then retry
hal.scheduler->delay_microseconds(500); hal.scheduler->delay_microseconds(500);
trigger_event(event); chEvtSignal(thread_ctx, event_mask);
} }
/* /*
@ -113,11 +113,12 @@ void AP_IOMCU::thread_main(void)
if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) { if (mask & EVENT_MASK(IOEVENT_SEND_PWM_OUT)) {
send_servo_out(); send_servo_out();
} }
mask &= ~EVENT_MASK(IOEVENT_SEND_PWM_OUT);
if (mask & EVENT_MASK(IOEVENT_INIT)) { if (mask & EVENT_MASK(IOEVENT_INIT)) {
// get protocol version // get protocol version
if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) { if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) {
event_failed(IOEVENT_INIT); event_failed(mask);
continue; continue;
} }
is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION && is_chibios_backend = (config.protocol_version == IOMCU_PROTOCOL_VERSION &&
@ -128,84 +129,94 @@ void AP_IOMCU::thread_main(void)
P_SETUP_ARMING_IO_ARM_OK | P_SETUP_ARMING_IO_ARM_OK |
P_SETUP_ARMING_FMU_ARMED | P_SETUP_ARMING_FMU_ARMED |
P_SETUP_ARMING_RC_HANDLING_DISABLED)) { P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
event_failed(IOEVENT_INIT); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_INIT);
if (mask & EVENT_MASK(IOEVENT_MIXING)) { if (mask & EVENT_MASK(IOEVENT_MIXING)) {
if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) { if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) {
event_failed(IOEVENT_MIXING); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_MIXING);
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) { if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
event_failed(IOEVENT_FORCE_SAFETY_OFF); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF);
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) { if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)) {
event_failed(IOEVENT_FORCE_SAFETY_ON); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_ON);
if (mask & EVENT_MASK(IOEVENT_SET_RATES)) { if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq) || if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq) ||
!write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask)) { !write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask)) {
event_failed(IOEVENT_SET_RATES); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_RATES);
if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) { if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, rate.sbus_rate_hz) || if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, rate.sbus_rate_hz) ||
!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, !modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0,
P_SETUP_FEATURES_SBUS1_OUT)) { P_SETUP_FEATURES_SBUS1_OUT)) {
event_failed(IOEVENT_ENABLE_SBUS); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_ENABLE_SBUS);
if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET)) { if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)) {
event_failed(IOEVENT_SET_HEATER_TARGET); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_HEATER_TARGET);
if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) { if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)) {
event_failed(IOEVENT_SET_DEFAULT_RATE); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_DEFAULT_RATE);
if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) { if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) { if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
event_failed(IOEVENT_SET_ONESHOT_ON); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_ONESHOT_ON);
if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) { if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) { if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) {
event_failed(IOEVENT_SET_BRUSHED_ON); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON);
if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) { if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) { if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
event_failed(IOEVENT_SET_SAFETY_MASK); event_failed(mask);
continue; continue;
} }
} }
mask &= ~EVENT_MASK(IOEVENT_SET_SAFETY_MASK);
// check for regular timed events // check for regular timed events
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
@ -739,12 +750,14 @@ void AP_IOMCU::set_default_rate(uint16_t rate_hz)
void AP_IOMCU::set_oneshot_mode(void) void AP_IOMCU::set_oneshot_mode(void)
{ {
trigger_event(IOEVENT_SET_ONESHOT_ON); trigger_event(IOEVENT_SET_ONESHOT_ON);
rate.oneshot_enabled = true;
} }
// setup for brushed mode // setup for brushed mode
void AP_IOMCU::set_brushed_mode(void) void AP_IOMCU::set_brushed_mode(void)
{ {
trigger_event(IOEVENT_SET_BRUSHED_ON); trigger_event(IOEVENT_SET_BRUSHED_ON);
rate.brushed_enabled = true;
} }
// handling of BRD_SAFETYOPTION parameter // handling of BRD_SAFETYOPTION parameter
@ -1045,6 +1058,18 @@ void AP_IOMCU::check_iomcu_reset(void)
} }
trigger_event(IOEVENT_SET_RATES); trigger_event(IOEVENT_SET_RATES);
trigger_event(IOEVENT_SET_DEFAULT_RATE); trigger_event(IOEVENT_SET_DEFAULT_RATE);
if (rate.oneshot_enabled) {
trigger_event(IOEVENT_SET_ONESHOT_ON);
}
if (rate.brushed_enabled) {
trigger_event(IOEVENT_SET_BRUSHED_ON);
}
if (rate.sbus_rate_hz) {
trigger_event(IOEVENT_ENABLE_SBUS);
}
if (pwm_out.safety_mask) {
trigger_event(IOEVENT_SET_SAFETY_MASK);
}
last_rc_protocols = 0; last_rc_protocols = 0;
} }

4
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -159,7 +159,7 @@ private:
void read_servo(void); void read_servo(void);
void read_status(void); void read_status(void);
void discard_input(void); void discard_input(void);
void event_failed(uint8_t event); void event_failed(uint32_t event_mask);
void update_safety_options(void); void update_safety_options(void);
void send_rc_protocols(void); void send_rc_protocols(void);
@ -201,6 +201,8 @@ private:
uint16_t chmask; uint16_t chmask;
uint16_t default_freq = 50; uint16_t default_freq = 50;
uint16_t sbus_rate_hz; uint16_t sbus_rate_hz;
bool oneshot_enabled;
bool brushed_enabled;
} rate; } rate;
// IMU heater duty cycle // IMU heater duty cycle