Copter: integrate RC_Channel's RC_FS_TIMEOUT

timeout is lengthened for regular RC to 1 sec (was 0.5 sec) to be
consistent with other vehicles
This commit is contained in:
Randy Mackay 2022-11-14 16:59:35 +09:00
parent 14f729babb
commit dd8fbad653

View File

@ -113,10 +113,9 @@ void Copter::read_radio()
return; return;
} }
const uint32_t elapsed = tnow_ms - last_radio_update_ms; // trigger failsafe if no update from the RC Radio for RC_FS_TIMEOUT seconds
// turn on throttle failsafe if no update from the RC Radio for 500ms or 1000ms if we are using RC_OVERRIDE const uint32_t elapsed_ms = tnow_ms - last_radio_update_ms;
const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS; if (elapsed_ms < rc().get_fs_timeout_ms()) {
if (elapsed < timeout) {
// not timed out yet // not timed out yet
return; return;
} }
@ -129,7 +128,7 @@ void Copter::read_radio()
return; return;
} }
// Nobody ever talks to us. Log an error and enter failsafe. // Log an error and enter failsafe.
AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
set_failsafe_radio(true); set_failsafe_radio(true);
} }