fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 14:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

enable disarmed PWM and rework safety switch disable logic

Browse Source
This commit is contained in:
Mark Whitehorn
2016-02-29 12:29:30 -07:00
parent 29b0520263
commit fcfe64ee5a
1 changed files with 21 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+21 -19
View File
@@ -1065,36 +1065,38 @@ PX4FMU::cycle()
if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) {
_last_safety_check = _cycle_timestamp; _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)) { if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
/* safety switch disabled, turn LED on solid */ /* safety switch disabled, turn LED on solid */
stm32_gpiowrite(GPIO_LED_SAFETY, 0); stm32_gpiowrite(GPIO_LED_SAFETY, 0);
_safety_off = true;
} else { } else {
/* read safety switch input at 10Hz */ /* read safety switch input and control safety switch LED at 10Hz */
safety_check_button(); safety_check_button();
}
/** safety.timestamp = hrt_absolute_time();
* Get and handle the safety status
*/
struct safety_s safety;
safety.timestamp = hrt_absolute_time();
if (_safety_off) { if (_safety_off) {
safety.safety_off = true; safety.safety_off = true;
safety.safety_switch_available = true; safety.safety_switch_available = true;
} else { } else {
safety.safety_off = false; safety.safety_off = false;
safety.safety_switch_available = true; safety.safety_switch_available = true;
} }
/* lazily publish the safety status */ /* lazily publish the safety status */
if (_to_safety != nullptr) { if (_to_safety != nullptr) {
orb_publish(ORB_ID(safety), _to_safety, &safety); orb_publish(ORB_ID(safety), _to_safety, &safety);
} else { } else {
_to_safety = orb_advertise(ORB_ID(safety), &safety); _to_safety = orb_advertise(ORB_ID(safety), &safety);
}
} }
} }