Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues 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
*/
void AP_IOMCU::event_failed(uint8_t event)
void AP_IOMCU::event_failed(uint32_t event_mask)
{
// wait 0.5ms then retry
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)) {
send_servo_out();
}
mask &= ~EVENT_MASK(IOEVENT_SEND_PWM_OUT);
if (mask & EVENT_MASK(IOEVENT_INIT)) {
// get protocol version
if (!read_registers(PAGE_CONFIG, 0, sizeof(config)/2, (uint16_t *)&config)) {
event_failed(IOEVENT_INIT);
event_failed(mask);
continue;
}
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_FMU_ARMED |
P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
event_failed(IOEVENT_INIT);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_INIT);
if (mask & EVENT_MASK(IOEVENT_MIXING)) {
if (!write_registers(PAGE_MIXING, 0, sizeof(mixing)/2, (const uint16_t *)&mixing)) {
event_failed(IOEVENT_MIXING);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_MIXING);
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_OFF, FORCE_SAFETY_MAGIC)) {
event_failed(IOEVENT_FORCE_SAFETY_OFF);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF);
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_FORCE_SAFETY_ON, FORCE_SAFETY_MAGIC)) {
event_failed(IOEVENT_FORCE_SAFETY_ON);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_FORCE_SAFETY_ON);
if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq) ||
!write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask)) {
event_failed(IOEVENT_SET_RATES);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_RATES);
if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_SBUS_RATE, rate.sbus_rate_hz) ||
!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0,
P_SETUP_FEATURES_SBUS1_OUT)) {
event_failed(IOEVENT_ENABLE_SBUS);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_ENABLE_SBUS);
if (mask & EVENT_MASK(IOEVENT_SET_HEATER_TARGET)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, heater_duty_cycle)) {
event_failed(IOEVENT_SET_HEATER_TARGET);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_HEATER_TARGET);
if (mask & EVENT_MASK(IOEVENT_SET_DEFAULT_RATE)) {
if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_DEFAULTRATE, rate.default_freq)) {
event_failed(IOEVENT_SET_DEFAULT_RATE);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_DEFAULT_RATE);
if (mask & EVENT_MASK(IOEVENT_SET_ONESHOT_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_ONESHOT)) {
event_failed(IOEVENT_SET_ONESHOT_ON);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_ONESHOT_ON);
if (mask & EVENT_MASK(IOEVENT_SET_BRUSHED_ON)) {
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_FEATURES, 0, P_SETUP_FEATURES_BRUSHED)) {
event_failed(IOEVENT_SET_BRUSHED_ON);
event_failed(mask);
continue;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON);
if (mask & EVENT_MASK(IOEVENT_SET_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;
}
}
mask &= ~EVENT_MASK(IOEVENT_SET_SAFETY_MASK);
// check for regular timed events
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)
{
trigger_event(IOEVENT_SET_ONESHOT_ON);
rate.oneshot_enabled = true;
}
// setup for brushed mode
void AP_IOMCU::set_brushed_mode(void)
{
trigger_event(IOEVENT_SET_BRUSHED_ON);
rate.brushed_enabled = true;
}
// 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_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;
}

4
libraries/AP_IOMCU/AP_IOMCU.h
View File

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