mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Blimp: run prearm checks on all vehicles @1Hz, displaying @0.0333Hz
This commit is contained in:
parent
efe1dfe0f4
commit
2f7df9ef4c
@ -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);
|
||||||
|
@ -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;
|
||||||
|
@ -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();
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user