Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Rover: fixup cpu failsafe

Browse Source 
remove copying of rc input to servo output
disarm after 2 seconds
This commit is contained in:
khancyr 2017-08-22 11:26:13 +02:00 committed by Randy Mackay
parent b1516cc6e8
commit 2be28ddb1e
1 changed files with 5 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
APMrover2/failsafe.cpp
View File

@ -18,34 +18,24 @@ void Rover::failsafe_check()
{ {
static uint16_t last_mainLoop_count; static uint16_t last_mainLoop_count;
static uint32_t last_timestamp; static uint32_t last_timestamp;
static bool in_failsafe;
const uint32_t tnow = AP_HAL::micros(); const uint32_t tnow = AP_HAL::micros();
if (mainLoop_count != last_mainLoop_count) { if (mainLoop_count != last_mainLoop_count) {
// the main loop is running, all is OK // the main loop is running, all is OK
last_mainLoop_count = mainLoop_count; last_mainLoop_count = mainLoop_count;
last_timestamp = tnow; last_timestamp = tnow;
in_failsafe = false;
return; return;
} }
if (tnow - last_timestamp > 200000) { if (tnow - last_timestamp > 200000) {
// we have gone at least 0.2 seconds since the main loop // we have gone at least 0.2 seconds since the main loop
// ran. That means we're in trouble, or perhaps are in // ran. That means we're in trouble, or perhaps are in
// an initialisation routine or log erase. Start passing RC // an initialisation routine or log erase. disarm the motors
// inputs through to outputs // To-Do: log error to dataflash
in_failsafe = true; if (arming.is_armed()) {
} // disarm motors
disarm_motors();
if (in_failsafe && tnow - last_timestamp > 20000 &&
channel_throttle->read() >= static_cast<uint16_t>(g.fs_throttle_value)) {
// pass RC inputs to outputs every 20ms
last_timestamp = tnow;
hal.rcin->clear_overrides();
for (uint8_t ch = 0; ch < 4; ch++) {
hal.rcout->write(ch, hal.rcin->read(ch));
} }
SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
} }
} }