From f1a023c8723a87059207ff30c88bcc40af8c185f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 20 Feb 2019 12:52:55 +0900
Subject: [PATCH] AP_AHRS: add attitudes_consistent pre-arm check

---
 libraries/AP_AHRS/AP_AHRS.h          |  3 ++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 53 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  3 ++
 3 files changed, 59 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 34afebc331..724b2eba75 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -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;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 75180f1aab..ad60fb5697 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -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
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 169ad03792..83cb34f587 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -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;