From fcfe64ee5a0256dbb050f9a1eb26557fc3ba350e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 29 Feb 2016 12:28:39 -0700 Subject: [PATCH] enable disarmed PWM and rework safety switch disable logic --- src/drivers/px4fmu/fmu.cpp | 40 ++++++++++++++++++++------------------ 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bfa0b047a9..b95e28346f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1065,36 +1065,38 @@ PX4FMU::cycle() if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { _last_safety_check = _cycle_timestamp; + /** + * Get and handle the safety status at 10Hz + */ + struct safety_s safety = {}; + if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { /* safety switch disabled, turn LED on solid */ stm32_gpiowrite(GPIO_LED_SAFETY, 0); + _safety_off = true; } else { - /* read safety switch input at 10Hz */ + /* read safety switch input and control safety switch LED at 10Hz */ safety_check_button(); + } - /** - * Get and handle the safety status - */ - struct safety_s safety; - safety.timestamp = hrt_absolute_time(); + safety.timestamp = hrt_absolute_time(); - if (_safety_off) { - safety.safety_off = true; - safety.safety_switch_available = true; + if (_safety_off) { + safety.safety_off = true; + safety.safety_switch_available = true; - } else { - safety.safety_off = false; - safety.safety_switch_available = true; - } + } else { + safety.safety_off = false; + safety.safety_switch_available = true; + } - /* lazily publish the safety status */ - if (_to_safety != nullptr) { - orb_publish(ORB_ID(safety), _to_safety, &safety); + /* lazily publish the safety status */ + if (_to_safety != nullptr) { + orb_publish(ORB_ID(safety), _to_safety, &safety); - } else { - _to_safety = orb_advertise(ORB_ID(safety), &safety); - } + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); } }