fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 01:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

disable safety switch if CBRK_IO_SAFETY is on

Browse Source
This commit is contained in:
Mark Whitehorn
2016-02-27 11:25:56 +01:00
committed by Lorenz Meier
parent beb9707933
commit 04a9eada1e
1 changed files with 29 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+29 -22
View File
@@ -93,6 +93,8 @@
# include <systemlib/ppm_decode.h> # include <systemlib/ppm_decode.h>
#endif #endif
#include <systemlib/circuit_breaker.h>
#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ #define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */
#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ #define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
@@ -968,7 +970,7 @@ PX4FMU::cycle()
/* main outputs */ /* main outputs */
if (i == 0) { if (i == 0) {
// main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250; // main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250;
warnx("lat: %llu", hrt_absolute_time() - _controls[i].timestamp); // warnx("lat: %llu", hrt_absolute_time() - _controls[i].timestamp);
/* do only correct within the current phase */ /* do only correct within the current phase */
if (abs(main_out_latency) > SCHEDULE_INTERVAL) { if (abs(main_out_latency) > SCHEDULE_INTERVAL) {
@@ -1051,7 +1053,6 @@ PX4FMU::cycle()
} }
} }
/* read safety switch input at 10Hz */
_cycle_timestamp = hrt_absolute_time(); _cycle_timestamp = hrt_absolute_time();
#ifdef GPIO_BTN_SAFETY #ifdef GPIO_BTN_SAFETY
@@ -1059,30 +1060,36 @@ 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;
/* check safety switch */ if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
safety_check_button(); /* safety switch disabled, turn LED on solid */
stm32_gpiowrite(GPIO_LED_SAFETY, 0);
/**
* Get and handle the safety status
*/
struct safety_s safety;
safety.timestamp = hrt_absolute_time();
if (_safety_off) {
safety.safety_off = true;
safety.safety_switch_available = true;
} else { } else {
safety.safety_off = false; /* read safety switch input at 10Hz */
safety.safety_switch_available = true; safety_check_button();
}
/* lazily publish the safety status */ /**
if (_to_safety != nullptr) { * Get and handle the safety status
orb_publish(ORB_ID(safety), _to_safety, &safety); */
struct safety_s safety;
safety.timestamp = hrt_absolute_time();
} else { if (_safety_off) {
_to_safety = orb_advertise(ORB_ID(safety), &safety); safety.safety_off = true;
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);
} else {
_to_safety = orb_advertise(ORB_ID(safety), &safety);
}
} }
} }