zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_IOMCU: fixed event handling bug in IOMCU code 

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
c415-sdk
Andrew Tridgell 4 years ago
parent
9126579b77
commit
9d87645c6e
2 changed files with 43 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 55
      libraries/AP_IOMCU/AP_IOMCU.cpp
  2. 4
      libraries/AP_IOMCU/AP_IOMCU.h

55
libraries/AP_IOMCU/AP_IOMCU.cpp
Unescape View File

@ -85,11 +85,11 @@ void AP_IOMCU::init(void) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -159,7 +159,7 @@ private: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save