Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: added retries on event failure

Browse Source
This commit is contained in:
Andrew Tridgell 2018-01-02 17:59:20 +11:00
parent 9b9b4a169e
commit 1568f578d7
2 changed files with 44 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -123,6 +123,16 @@ void AP_IOMCU::thread_start(void *ctx)
((AP_IOMCU *)ctx)->thread_main();
}
/*
handle event failure
*/
void AP_IOMCU::event_failed(uint8_t event)
{
// wait 0.5ms then retry
hal.scheduler->delay_microseconds(500);
trigger_event(event);
}
/*
main IO thread loop
*/
@ -132,12 +142,8 @@ void AP_IOMCU::thread_main(void)
uart.begin(1500*1000, 256, 256);
uart.set_blocking_writes(false);
// set IO_ARM_OK and FMU_ARMED
modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
P_SETUP_ARMING_IO_ARM_OK |
P_SETUP_ARMING_FMU_ARMED |
P_SETUP_ARMING_RC_HANDLING_DISABLED);
trigger_event(IOEVENT_INIT);
// enable sbus (until we have BRD_SBUS_OUT available in ChibiOS)
enable_sbus_out(150);
@ -149,24 +155,48 @@ void AP_IOMCU::thread_main(void)
send_servo_out();
}
if (mask & EVENT_MASK(IOEVENT_INIT)) {
// set IO_ARM_OK and FMU_ARMED
if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
P_SETUP_ARMING_IO_ARM_OK |
P_SETUP_ARMING_FMU_ARMED |
P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
event_failed(IOEVENT_INIT);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_OFF)) {
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);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_FORCE_SAFETY_ON)) {
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);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_SET_RATES)) {
write_register(PAGE_SETUP, PAGE_REG_SETUP_ALTRATE, rate.freq);
write_register(PAGE_SETUP, PAGE_REG_SETUP_PWM_RATE_MASK, rate.chmask);
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);
continue;
}
}
if (mask & EVENT_MASK(IOEVENT_ENABLE_SBUS)) {
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);
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);
continue;
}
}
// check for regular timed events

1
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -102,6 +102,7 @@ private:
void read_status(void);
void print_debug(void);
void discard_input(void);
void event_failed(uint8_t event);
static const uint8_t max_channels = 16;