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

AP_AHRS: add attitudes_consistent pre-arm check

Browse Source
This commit is contained in:
Randy Mackay 2019-02-20 12:52:55 +09:00
parent 07f7d793df
commit f1a023c872
3 changed files with 59 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -216,6 +216,9 @@ public:
return nullptr;
}
// check all cores providing consistent attitudes for prearm checks
virtual bool attitudes_consistent() const { return true; }
// is the EKF backend doing its own sensor logging?
virtual bool have_ekf_logging(void) const {
return false;

53
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -27,6 +27,9 @@
#if AP_AHRS_NAVEKF_AVAILABLE
#define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
#define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
extern const AP_HAL::HAL& hal;
// constructor
@ -1310,6 +1313,56 @@ const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const
return nullptr;
}
// check all cores providing consistent attitudes for prearm checks
bool AP_AHRS_NavEKF::attitudes_consistent() const
{
// get primary attitude source's attitude as quaternion
Quaternion primary_quat;
primary_quat.from_euler(roll, pitch, yaw);
// check primary vs ekf2
for (uint8_t i = 0; i < EKF2.activeCores(); i++) {
Quaternion ekf2_quat;
Vector3f angle_diff;
EKF2.getQuaternion(i, ekf2_quat);
primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
if (safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)) > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
return false;
}
}
// check primary vs ekf3
for (uint8_t i = 0; i < EKF3.activeCores(); i++) {
Quaternion ekf3_quat;
Vector3f angle_diff;
EKF3.getQuaternion(i, ekf3_quat);
primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
if (safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)) > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
return false;
}
}
// check primary vs dcm
Quaternion dcm_quat;
Vector3f angle_diff;
dcm_quat.from_axis_angle(_dcm_attitude);
primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
if (safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)) > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
return false;
}
return true;
}
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) const

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -188,6 +188,9 @@ public:
// 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() const override;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng) const override;