From 39ebe6150fed378ef08591a3667ca8a4a698a4b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 24 Aug 2023 14:49:59 +1000 Subject: [PATCH] AP_ExternalAHRS: added CINS backend --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 23 +++++- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 13 +++- .../AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp | 75 +++++++++++++++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_CINS.h | 58 ++++++++++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 2 +- .../AP_ExternalAHRS/AP_ExternalAHRS_config.h | 4 + 6 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 9abbfff0d1a97c..9c56a3030c043d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/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 #include @@ -89,6 +90,13 @@ 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 }; @@ -96,6 +104,9 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { 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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 22ef0e9d5f8299..77e02e846d4f46 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -27,6 +27,10 @@ #include #include +#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 { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp new file mode 100644 index 00000000000000..53f414702e869f --- /dev/null +++ b/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 . + */ + +#define ALLOW_DOUBLE_MATH_FUNCTIONS + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_CINS_ENABLED + +#include "AP_ExternalAHRS_CINS.h" +#include + +// 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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h new file mode 100644 index 00000000000000..98ba29da5b35fe --- /dev/null +++ b/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 . + */ +/* + CINS external AHRS implementation + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_CINS_ENABLED + +#include "AP_ExternalAHRS_backend.h" +#include + +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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index e5a22a87cbe44b..cc37667a7f8d3c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/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; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 650f605b0a9f22..235f37f7b3b0ac 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/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