mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
ee073787c8
Instead of going through 'hal' then 'scheduler', use directly the AP_HAL
functions. Besides removing indirection that is not necessary for such
functions, this patch ends up reducing the code size in the call sites.
For example, building ArduCopter for PX4 with this change (compared to
before introduction of the functions) yields almost 3k bytes of code
size.
# ArduCopter build before the functions (1b29a1af46)
text data bss dec hex filename
895264 2812 62732 960808 ea928 /.../px4fmu-v2_APM.build/firmware.elf
# ArduCopter build after this patch
text data bss dec hex filename
892264 2812 62732 957808 e9d70 /.../px4fmu-v2_APM.build/firmware.elf
A later patch will remove the unused functions in the Schedulers.
54 lines
1.5 KiB
C++
54 lines
1.5 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
failsafe support
|
|
Andrew Tridgell, December 2011
|
|
*/
|
|
|
|
#include "Rover.h"
|
|
|
|
/*
|
|
our failsafe strategy is to detect main loop lockup and switch to
|
|
passing inputs straight from the RC inputs to RC outputs.
|
|
*/
|
|
|
|
/*
|
|
this failsafe_check function is called from the core timer interrupt
|
|
at 1kHz.
|
|
*/
|
|
void Rover::failsafe_check()
|
|
{
|
|
static uint16_t last_mainLoop_count;
|
|
static uint32_t last_timestamp;
|
|
static bool in_failsafe;
|
|
uint32_t tnow = AP_HAL::micros();
|
|
|
|
if (mainLoop_count != last_mainLoop_count) {
|
|
// the main loop is running, all is OK
|
|
last_mainLoop_count = mainLoop_count;
|
|
last_timestamp = tnow;
|
|
in_failsafe = false;
|
|
return;
|
|
}
|
|
|
|
if (tnow - last_timestamp > 200000) {
|
|
// we have gone at least 0.2 seconds since the main loop
|
|
// ran. That means we're in trouble, or perhaps are in
|
|
// an initialisation routine or log erase. Start passing RC
|
|
// inputs through to outputs
|
|
in_failsafe = true;
|
|
}
|
|
|
|
if (in_failsafe && tnow - last_timestamp > 20000 &&
|
|
channel_throttle->read() >= (uint16_t)g.fs_throttle_value) {
|
|
// pass RC inputs to outputs every 20ms
|
|
last_timestamp = tnow;
|
|
hal.rcin->clear_overrides();
|
|
uint8_t start_ch = 0;
|
|
for (uint8_t ch=start_ch; ch<4; ch++) {
|
|
hal.rcout->write(ch, hal.rcin->read(ch));
|
|
}
|
|
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
|
|
}
|
|
}
|
|
|