diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-03 11:45:57 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-03 11:45:57 +0400 |
commit | 367ce63b86b863d72a3f6b4250d9da780a85f40b (patch) | |
tree | c462d06d347941d3a01791f17e62053bbc75f44e /src/modules/commander | |
parent | 0205eebaa63016b3cf4bb03a5af230554d4a581b (diff) | |
download | px4-firmware-367ce63b86b863d72a3f6b4250d9da780a85f40b.tar.gz px4-firmware-367ce63b86b863d72a3f6b4250d9da780a85f40b.tar.bz2 px4-firmware-367ce63b86b863d72a3f6b4250d9da780a85f40b.zip |
'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 13 |
1 files changed, 2 insertions, 11 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..edd77231d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && !sp_man.signal_lost && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; |