aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: replace prearm_healthy with pre_arm_check

Browse Source 
Also removes prearm_failure_reason
This commit is contained in:
Randy Mackay 2020-08-11 14:02:55 +09:00 committed by Peter Barker
parent 02daa4c3ec
commit 1d97416a51
5 changed files with 35 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS.h
View File

@ -171,10 +171,8 @@ public:
// Methods
virtual void update(bool skip_ins_update=false) = 0;
// report any reason for why the backend is refusing to initialise
virtual const char *prearm_failure_reason(void) const {
return nullptr;
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0;
// check all cores providing consistent attitudes for prearm checks
virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; }
@ -482,8 +480,6 @@ public:
// is the AHRS subsystem healthy?
virtual bool healthy(void) const = 0;
virtual bool prearm_healthy(void) const { return healthy(); }
// true if the AHRS has completed initialisation
virtual bool initialised(void) const {
return true;

9
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1142,3 +1142,12 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const
return true;
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_AHRS_DCM::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy");
return false;
}
return true;
}

3
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -120,6 +120,9 @@ public:
bool get_velocity_NED(Vector3f &vec) const override;
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
private:
float _ki;
float _ki_yaw;

56
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1344,34 +1344,42 @@ bool AP_AHRS_NavEKF::healthy(void) const
return AP_AHRS_DCM::healthy();
}
bool AP_AHRS_NavEKF::prearm_healthy(void) const
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
bool prearm_health = false;
switch (ekf_type()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
#endif
case EKFType::NONE:
prearm_health = true;
break;
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy");
return false;
}
return true;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
if (_ekf2_started && EKF2.all_cores_healthy()) {
prearm_health = true;
if (!_ekf2_started) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 not started");
return false;
}
break;
return EKF2.pre_arm_check(failure_msg, failure_msg_len);
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
if (_ekf3_started && EKF3.all_cores_healthy()) {
prearm_health = true;
if (!_ekf3_started) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started");
return false;
}
break;
return EKF3.pre_arm_check(failure_msg, failure_msg_len);
#endif
}
return prearm_health && healthy();
// if we get here then ekf type is invalid
hal.util->snprintf(failure_msg, failure_msg_len, "invalid EKF type");
return false;
}
void AP_AHRS_NavEKF::set_ekf_use(bool setting)
@ -1617,32 +1625,6 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons
ret.z += GRAVITY_MSS*dt;
}
// report any reason for why the backend is refusing to initialise
const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const
{
switch (ekf_type()) {
case EKFType::NONE:
return nullptr;
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return EKF2.prearm_failure_reason();
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
return EKF3.prearm_failure_reason();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
return nullptr;
#endif
}
return nullptr;
}
// check all cores providing consistent attitudes for prearm checks
bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const
{

6
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -200,7 +200,8 @@ public:
// is the AHRS subsystem healthy?
bool healthy() const override;
bool prearm_healthy() const override;
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
// true if the AHRS has completed initialisation
bool initialised() const override;
@ -212,9 +213,6 @@ public:
// true if offsets are valid
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const override;
// check all cores providing consistent attitudes for prearm checks
bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const override;