Rover: don't do failsafe if throttle is below failsafe level

This commit is contained in:
Andrew Tridgell 2013-05-03 09:45:15 +10:00
parent 09ca9d4283
commit ef4198bcb4

View File

@ -35,7 +35,8 @@ void failsafe_check(uint32_t tnow)
in_failsafe = true;
}
if (in_failsafe && tnow - last_timestamp > 20000) {
if (in_failsafe && tnow - last_timestamp > 20000 &&
hal.rcin->read(CH_3) >= (uint16_t)g.fs_throttle_value) {
// pass RC inputs to outputs every 20ms
last_timestamp = tnow;
hal.rcin->clear_overrides();