Blimp: run prearm checks on all vehicles @1Hz, displaying @0.0333Hz

This commit is contained in:
Peter Barker 2022-05-10 17:53:32 +10:00 committed by Randy Mackay
parent efe1dfe0f4
commit 2f7df9ef4c
4 changed files with 0 additions and 24 deletions

View File

@ -1,21 +1,5 @@
#include "Blimp.h" #include "Blimp.h"
// performs pre-arm checks. expects to be called at 1hz.
void AP_Arming_Blimp::update(void)
{
// perform pre-arm checks & display failures every 30 seconds
static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
pre_arm_display_counter++;
bool display_fail = false;
if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) {
display_fail = true;
pre_arm_display_counter = 0;
}
pre_arm_checks(display_fail);
}
bool AP_Arming_Blimp::pre_arm_checks(bool display_failure) bool AP_Arming_Blimp::pre_arm_checks(bool display_failure)
{ {
const bool passed = run_pre_arm_checks(display_failure); const bool passed = run_pre_arm_checks(display_failure);

View File

@ -19,8 +19,6 @@ public:
AP_Arming_Blimp(const AP_Arming_Blimp &other) = delete; AP_Arming_Blimp(const AP_Arming_Blimp &other) = delete;
AP_Arming_Blimp &operator=(const AP_Arming_Blimp&) = delete; AP_Arming_Blimp &operator=(const AP_Arming_Blimp&) = delete;
void update(void);
bool rc_calibration_checks(bool display_failure) override; bool rc_calibration_checks(bool display_failure) override;
bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override; bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override;

View File

@ -204,8 +204,6 @@ void Blimp::one_hz_loop()
Log_Write_Data(LogDataID::AP_STATE, ap.value); Log_Write_Data(LogDataID::AP_STATE, ap.value);
} }
arming.update();
if (!motors->armed()) { if (!motors->armed()) {
// make it possible to change ahrs orientation at runtime during initial config // make it possible to change ahrs orientation at runtime during initial config
ahrs.update_orientation(); ahrs.update_orientation();

View File

@ -132,10 +132,6 @@
#define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL) #define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL)
#endif #endif
#ifndef PREARM_DISPLAY_PERIOD
# define PREARM_DISPLAY_PERIOD 30
#endif
// pre-arm baro vs inertial nav max alt disparity // pre-arm baro vs inertial nav max alt disparity
#ifndef PREARM_MAX_ALT_DISPARITY_CM #ifndef PREARM_MAX_ALT_DISPARITY_CM
# define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters