fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 14:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: lost signal detection rewritten to be more clear

Browse Source
This commit is contained in:
Anton Babushkin 2014-04-07 14:32:37 +04:00
parent 9a579fa870
commit eb5cd54023
1 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/sensors/sensors.cpp
View File

@ -1307,17 +1307,26 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
/* detect RC signal loss */
bool signal_lost = true;
bool signal_lost;
/* check flags and require at least four channels to consider the signal valid */
if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) {
/* signal looks good, but check for throttle failsafe */
if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
/* signal is lost or no enough channels */
signal_lost = true;
} else {
/* signal looks good */
signal_lost = false;
/* check throttle failsafe */
int8_t thr_ch = _rc.function[THROTTLE];
if (_parameters.rc_fs_thr == 0 || thr_ch < 0 ||
!((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) {
/* valid signal, throttle failsafe not configured or not triggered */
signal_lost = false;
if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
/* throttle failsafe configured */
if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
/* throttle failsafe triggered, signal is lost by receiver */
signal_lost = true;
}
}
}