Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: attitude pre-arm check skips yaw if no compass

Browse Source 
Also provides better feedback on the axis and angular difference
This commit is contained in:
Randy Mackay 2019-03-06 15:50:11 +09:00
parent 91436c5314
commit ba02894734
3 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.h
View File

@ -217,7 +217,7 @@ public:
}
// check all cores providing consistent attitudes for prearm checks
virtual bool attitudes_consistent() const { return true; }
virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; }
// is the EKF backend doing its own sensor logging?
virtual bool have_ekf_logging(void) const {

29
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1314,22 +1314,29 @@ const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const
}
// check all cores providing consistent attitudes for prearm checks
bool AP_AHRS_NavEKF::attitudes_consistent() const
bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const
{
// get primary attitude source's attitude as quaternion
Quaternion primary_quat;
primary_quat.from_euler(roll, pitch, yaw);
// only check yaw if compasses are being used
bool check_yaw = _compass && _compass->use_for_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) {
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Yaw inconsistent by %d deg", (int)degrees(diff));
return false;
}
}
@ -1340,10 +1347,14 @@ bool AP_AHRS_NavEKF::attitudes_consistent() const
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) {
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Yaw inconsistent by %d deg", (int)degrees(diff));
return false;
}
}
@ -1353,10 +1364,14 @@ bool AP_AHRS_NavEKF::attitudes_consistent() const
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) {
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
return false;
}
if (fabsf(angle_diff.z) > ATTITUDE_CHECK_THRESH_YAW_RAD) {
diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(diff));
return false;
}

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -189,7 +189,7 @@ public:
const char *prearm_failure_reason(void) const override;
// check all cores providing consistent attitudes for prearm checks
bool attitudes_consistent() const override;
bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) 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