Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: implement safety LED and switch in iofirmware

This commit is contained in:
Andrew Tridgell 2018-05-11 11:43:28 +10:00
parent 037a455784
commit ecfe06b9e4
2 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -139,6 +139,7 @@ void AP_IOMCU_FW::update()
pwm_out_update();
heater_update();
rcin_update();
safety_update();
}
void AP_IOMCU_FW::pwm_out_update()
@ -416,4 +417,39 @@ void AP_IOMCU_FW::calculate_fw_crc(void)
}
/*
update safety state
*/
void AP_IOMCU_FW::safety_update(void)
{
uint32_t now = AP_HAL::millis();
if (now - safety_update_ms < 100) {
// update safety at 10Hz
return;
}
safety_update_ms = now;
bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
if (safety_pressed) {
if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
safety_pressed = false;
} else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) {
safety_pressed = false;
}
}
if (safety_pressed) {
safety_button_counter++;
} else {
safety_button_counter = 0;
}
if (safety_button_counter == 10) {
// safety has been pressed for 1 second, change state
reg_status.flag_safety_off = !reg_status.flag_safety_off;
}
led_counter = (led_counter+1) % 16;
const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
}
AP_HAL_MAIN();

4
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -26,6 +26,7 @@ private:
bool handle_code_write();
bool handle_code_read();
void schedule_reboot(uint32_t time_ms);
void safety_update();
struct PACKED {
/* default to RSSI ADC functionality */
@ -87,5 +88,8 @@ private:
bool update_rcout_freq;
bool has_heater;
uint32_t last_blue_led_ms;
uint32_t safety_update_ms;
uint32_t safety_button_counter;
uint8_t led_counter;
};