yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00: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-08 13:31:31 -07: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

51
src/drivers/px4fmu/fmu.cpp
View File

@@ -93,6 +93,8 @@
# include <systemlib/ppm_decode.h>
#endif
#include <systemlib/circuit_breaker.h>
#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 BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
@@ -968,7 +970,7 @@ PX4FMU::cycle()
/* main outputs */
if (i == 0) {
// 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 */
if (abs(main_out_latency) > SCHEDULE_INTERVAL) {
@@ -1051,7 +1053,6 @@ PX4FMU::cycle()
}
}
/* read safety switch input at 10Hz */
_cycle_timestamp = hrt_absolute_time();
#ifdef GPIO_BTN_SAFETY
@@ -1059,30 +1060,36 @@ PX4FMU::cycle()
if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) {
_last_safety_check = _cycle_timestamp;
/* check safety switch */
safety_check_button();
/**
* 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;
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
/* safety switch disabled, turn LED on solid */
stm32_gpiowrite(GPIO_LED_SAFETY, 0);
} else {
safety.safety_off = false;
safety.safety_switch_available = true;
}
/* read safety switch input at 10Hz */
safety_check_button();
/* lazily publish the safety status */
if (_to_safety != nullptr) {
orb_publish(ORB_ID(safety), _to_safety, &safety);
/**
* Get and handle the safety status
*/
struct safety_s safety;
safety.timestamp = hrt_absolute_time();
} else {
_to_safety = orb_advertise(ORB_ID(safety), &safety);
if (_safety_off) {
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);
}
}
}