AP_ExternalAHRS: added CINS backend

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -26,6 +26,7 @@
 #include "AP_ExternalAHRS_MicroStrain5.h"
 #include "AP_ExternalAHRS_MicroStrain7.h"
 #include "AP_ExternalAHRS_InertialLabs.h"
+#include "AP_ExternalAHRS_CINS.h"
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -89,13 +90,23 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @Units: Hz
     // @User: Standard
     AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
+
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+    // @Group: _CINS_
+    // @Path: ../AP_CINS/AP_CINS.cpp
+    AP_SUBGROUPPTR(cins_ptr, "_CINS_", 6, AP_ExternalAHRS, AP_CINS),
+#endif
+
 
     AP_GROUPEND
 };
 
 
 void AP_ExternalAHRS::init(void)
 {
+    if (backend != nullptr) {
+        return;
+    }
     if (rate.get() < 50) {
         // min 50Hz
         rate.set(50);
@@ -130,6 +141,12 @@ void AP_ExternalAHRS::init(void)
         return;
 #endif
 
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+    case DevType::CINS:
+        backend = new AP_ExternalAHRS_CINS(this, state, cins_ptr);
+        return;
+#endif
+
     }
 
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype));
@@ -292,7 +309,9 @@ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
 bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
 {
     WITH_SEMAPHORE(state.sem);
-    if (!has_sensor(AvailableSensor::IMU)) {
+    // use accel as a proxy for having gyro - we never expect an exactly
+    // zero accel, but may have a zero gyro
+    if (!has_sensor(AvailableSensor::IMU) || state.accel.is_zero()) {
         return false;
     }
     gyro = state.gyro;
@@ -302,7 +321,7 @@ bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
 bool AP_ExternalAHRS::get_accel(Vector3f &accel)
 {
     WITH_SEMAPHORE(state.sem);
-    if (!has_sensor(AvailableSensor::IMU)) {
+    if (!has_sensor(AvailableSensor::IMU) || state.accel.is_zero()) {
         return false;
     }
     accel = state.accel;

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -27,6 +27,10 @@
 #include <AP_Common/Location.h>
 #include <AP_NavEKF/AP_Nav_Common.h>
 
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+class AP_CINS;
+#endif
+
 class AP_ExternalAHRS_backend;
 
 class AP_ExternalAHRS {
@@ -49,11 +53,12 @@ class AP_ExternalAHRS {
 #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
         MicroStrain5 = 2,
 #endif
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+        CINS = 4,
+#endif
 #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
         InertialLabs = 5,
 #endif
-        // 3 reserved for AdNav
-        // 4 reserved for CINS
         // 6 reserved for Trimble
 #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
         MicroStrain7 = 7,
@@ -202,6 +207,10 @@ class AP_ExternalAHRS {
 
     // true when user has disabled the GNSS
     bool gnss_is_disabled;
+
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+    AP_CINS *cins_ptr;
+#endif
 };
 
 namespace AP {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp

@@ -0,0 +1,75 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define ALLOW_DOUBLE_MATH_FUNCTIONS
+
+#include "AP_ExternalAHRS_config.h"
+
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+
+#include "AP_ExternalAHRS_CINS.h"
+#include <GCS_MAVLink/GCS.h>
+
+// constructor
+AP_ExternalAHRS_CINS::AP_ExternalAHRS_CINS(AP_ExternalAHRS *_frontend,
+                                           AP_ExternalAHRS::state_t &_state,
+                                           AP_CINS *&cins_ptr) :
+    AP_ExternalAHRS_backend(_frontend, _state)
+{
+    cins_ptr = &cins;
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CINS Started");
+    cins.init();
+    // CINS does not provide new sensors, only a state estimate
+    set_default_sensors(0);
+}
+
+void AP_ExternalAHRS_CINS::update(void)
+{
+    cins.update();
+
+    WITH_SEMAPHORE(state.sem);
+
+    state.accel = cins.get_accel();
+    state.gyro = cins.get_gyro();
+    state.quat = cins.get_quat();
+    state.location = cins.get_location();
+    state.velocity = cins.get_velocity();
+    state.have_origin = cins.get_origin(state.origin);
+    state.have_quaternion = true;
+    state.have_location = state.have_origin;
+    state.have_velocity = true;
+}
+
+/*
+  get filter status, assume all OK if we have GPS lock
+ */
+void AP_ExternalAHRS_CINS::get_filter_status(nav_filter_status &status) const
+{
+    memset(&status, 0, sizeof(status));
+    if (cins.healthy()) {
+        status.flags.initalized = 1;
+        status.flags.attitude = 1;
+        status.flags.vert_vel = 1;
+        status.flags.vert_pos = 1;
+        status.flags.horiz_vel = 1;
+        status.flags.horiz_pos_rel = 1;
+        status.flags.horiz_pos_abs = 1;
+        status.flags.pred_horiz_pos_rel = 1;
+        status.flags.pred_horiz_pos_abs = 1;
+        status.flags.using_gps = 1;
+    }
+}
+
+#endif // AP_EXTERNAL_AHRS_CINS_ENABLED

libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h

@@ -0,0 +1,58 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  CINS external AHRS implementation
+ */
+
+#pragma once
+
+#include "AP_ExternalAHRS_config.h"
+
+#if AP_EXTERNAL_AHRS_CINS_ENABLED
+
+#include "AP_ExternalAHRS_backend.h"
+#include <AP_CINS/AP_CINS.h>
+
+class AP_ExternalAHRS_CINS : public AP_ExternalAHRS_backend {
+
+public:
+    AP_ExternalAHRS_CINS(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state, AP_CINS *&cins_ptr);
+
+    // accessors for AP_AHRS
+    bool healthy(void) const override {
+        return true;
+    }
+    bool initialised(void) const override {
+        return true;
+    }
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override {
+        return true;
+    }
+
+    void get_filter_status(nav_filter_status &status) const override;
+
+    // check for new data
+    void update() override;
+
+    // Get model/type name
+    const char* get_name() const override {
+        return "CINS";
+    }
+
+private:
+    AP_CINS cins;
+};
+
+#endif  // AP_EXTERNAL_AHRS_CINS_ENABLED

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -49,7 +49,7 @@ class AP_ExternalAHRS_backend {
     virtual void update() = 0;
 
     // Return the number of GPS sensors sharing data to AP_GPS.
-    virtual uint8_t num_gps_sensors(void) const = 0;
+    virtual uint8_t num_gps_sensors(void) const { return 0; }
 
 protected:
     AP_ExternalAHRS::state_t &state;

libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h

@@ -29,3 +29,7 @@
 #ifndef AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
 #define AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_EXTERNAL_AHRS_CINS_ENABLED
+#define AP_EXTERNAL_AHRS_CINS_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#endif