fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 20:19:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Proper failsafe handling onboard, including throttle failsafe condition if enabled

Browse Source
This commit is contained in:
Lorenz Meier 2014-04-05 10:27:43 +02:00
parent 409fa565f4
commit c6d98a32f8
2 changed files with 29 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/modules/px4iofirmware/controls.c
View File

@ -134,8 +134,6 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
@ -261,8 +259,20 @@ controls_tick() {
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
}
if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
@ -312,6 +322,11 @@ controls_tick() {
* Handle losing RC input
*/
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
@ -322,27 +337,24 @@ controls_tick() {
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set raw channel count to zero */
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* must have R/C input (NO FAILSAFE!).
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;

9
src/modules/px4iofirmware/protocol.h
View File

@ -164,10 +164,11 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */