Microstrain7 Initial Support

Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	50.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367333	149.163164	28.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366814	149.165878	28.000000	1
+5	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362947	149.165179	0.000000	1

Tools/autotest/arduplane.py

@@ -3275,6 +3275,10 @@ def MicroStrainEAHRS5(self):
         '''Test MicroStrain EAHRS series 5 support'''
         self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt")
 
+    def MicroStrainEAHRS7(self):
+        '''Test MicroStrain EAHRS series 7 support'''
+        self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt")
+
     def get_accelvec(self, m):
         return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81
 
@@ -5353,6 +5357,7 @@ def tests(self):
             self.TerrainLoiter,
             self.VectorNavEAHRS,
             self.MicroStrainEAHRS5,
+            self.MicroStrainEAHRS7,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,
             self.EKFlaneswitch,

Tools/scripts/build_options.py

@@ -32,6 +32,7 @@ def __init__(self,
     Feature('AHRS', 'EKF2', 'HAL_NAVEKF2_AVAILABLE', 'Enable EKF2', 0, None),
     Feature('AHRS', 'AHRS_EXT', 'HAL_EXTERNAL_AHRS_ENABLED', 'Enable External AHRS', 0, None),
     Feature('AHRS', 'AHRS_EXT_MICROSTRAIN5', 'AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED', 'Enable MICROSTRAIN 5-series External AHRS', 0, "AHRS_EXT"),  # noqa: E501
+    Feature('AHRS', 'AHRS_EXT_MICROSTRAIN7', 'AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED', 'Enable MICROSTRAIN 7-series External AHRS', 0, "AHRS_EXT"),  # noqa: E501
     Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav External AHRS', 0, "AHRS_EXT"),  # noqa
     Feature('AHRS', 'TEMPCAL', 'HAL_INS_TEMPERATURE_CAL_ENABLE', 'Enable IMU Temperature Calibration', 0, None),
     Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, 'EKF3_EXTNAV'),

Tools/scripts/run_astyle.py

@@ -22,9 +22,14 @@ def __init__(self, *, dry_run=DRY_RUN_DEFAULT):
         self.retcode = 0
         self.directories_to_check = [
             'libraries/AP_DDS',
-            'libraries/AP_ExternalControl'
+            'libraries/AP_ExternalControl',
+        ]
+        self.files_to_check = [
+            pathlib.Path(s) for s in [
+                'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp',
+                'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h',
+            ]
         ]
-        self.files_to_check = []
         self.dry_run = dry_run
 
     def progress(self, string):

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -24,6 +24,7 @@
 #include "AP_ExternalAHRS_backend.h"
 #include "AP_ExternalAHRS_VectorNav.h"
 #include "AP_ExternalAHRS_MicroStrain5.h"
+#include "AP_ExternalAHRS_MicroStrain7.h"
 
 #include <GCS_MAVLink/GCS.h>
 
@@ -53,7 +54,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: AHRS type
     // @Description: Type of AHRS device
-    // @Values: 0:None,1:VectorNav,2:MicroStrain
+    // @Values: 0:None,1:VectorNav,2:MicroStrain5,7:MicroStrain7
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE),
 
@@ -102,6 +103,11 @@ void AP_ExternalAHRS::init(void)
     case DevType::MicroStrain5:
         backend = new AP_ExternalAHRS_MicroStrain5(this, state);
         return;
+#endif
+#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
+    case DevType::MicroStrain7:
+        backend = new AP_ExternalAHRS_MicroStrain7(this, state);
+        return;
 #endif
     }
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -53,7 +53,9 @@ class AP_ExternalAHRS {
         // 4 reserved for CINS
         // 5 reserved for InertialLabs
         // 6 reserved for Trimble
-        // 7 reserved for MicroStrain7
+#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
+        MicroStrain7 = 7,
+#endif
         // 8 reserved for SBG
         // 9 reserved for EulerNav
     };

libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp

@@ -34,6 +34,8 @@
 
 extern const AP_HAL::HAL &hal;
 
+static constexpr uint8_t gnss_instance = 0;
+
 AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_frontend,
         AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
 {
@@ -44,16 +46,16 @@ AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_fro
     port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
 
     if (!uart) {
-        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS no UART");
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain5 ExternalAHRS no UART");
         return;
     }
 
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain5::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
-        AP_BoardConfig::allocation_error("Failed to allocate ExternalAHRS update thread");
+        AP_BoardConfig::allocation_error("MicroStrain5 failed to allocate ExternalAHRS update thread");
     }
 
     hal.scheduler->delay(5000);
-    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised");
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain5 ExternalAHRS initialised");
 }
 
 void AP_ExternalAHRS_MicroStrain5::update_thread(void)
@@ -92,8 +94,10 @@ void AP_ExternalAHRS_MicroStrain5::build_packet()
                 post_imu();
                 break;
             case DescriptorSet::GNSSData:
+            case DescriptorSet::GNSSRecv1:
+            case DescriptorSet::GNSSRecv2:
                 break;
-            case DescriptorSet::EstimationData:
+            case DescriptorSet::FilterData:
                 post_filter();
                 break;
             case DescriptorSet::BaseCommand:
@@ -157,26 +161,26 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const
         state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
         state.have_velocity = true;
 
-        state.location = Location{filter_data.lat, filter_data.lon, gnss_data.msl_altitude, Location::AltFrame::ABSOLUTE};
+        state.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].msl_altitude, Location::AltFrame::ABSOLUTE};
         state.have_location = true;
     }
 
     AP_ExternalAHRS::gps_data_message_t gps {
         gps_week: filter_data.week,
         ms_tow: filter_data.tow_ms,
-        fix_type: (uint8_t) gnss_data.fix_type,
-        satellites_in_view: gnss_data.satellites,
+        fix_type: (uint8_t) gnss_data[gnss_instance].fix_type,
+        satellites_in_view: gnss_data[gnss_instance].satellites,
 
-        horizontal_pos_accuracy: gnss_data.horizontal_position_accuracy,
-        vertical_pos_accuracy: gnss_data.vertical_position_accuracy,
-        horizontal_vel_accuracy: gnss_data.speed_accuracy,
+        horizontal_pos_accuracy: gnss_data[gnss_instance].horizontal_position_accuracy,
+        vertical_pos_accuracy: gnss_data[gnss_instance].vertical_position_accuracy,
+        horizontal_vel_accuracy: gnss_data[gnss_instance].speed_accuracy,
 
-        hdop: gnss_data.hdop,
-        vdop: gnss_data.vdop,
+        hdop: gnss_data[gnss_instance].hdop,
+        vdop: gnss_data[gnss_instance].vdop,
 
         longitude: filter_data.lon,
         latitude: filter_data.lat,
-        msl_altitude: gnss_data.msl_altitude,
+        msl_altitude: gnss_data[gnss_instance].msl_altitude,
 
         ned_vel_north: filter_data.ned_velocity_north,
         ned_vel_east: filter_data.ned_velocity_east,
@@ -187,14 +191,14 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const
         WITH_SEMAPHORE(state.sem);
         state.origin = Location{int32_t(filter_data.lat),
                                 int32_t(filter_data.lon),
-                                int32_t(gnss_data.msl_altitude),
+                                int32_t(gnss_data[gnss_instance].msl_altitude),
                                 Location::AltFrame::ABSOLUTE};
         state.have_origin = true;
     }
 
-    uint8_t instance;
-    if (AP::gps().get_first_external_instance(instance)) {
-        AP::gps().handle_external(gps, instance);
+    uint8_t gps_instance;
+    if (AP::gps().get_first_external_instance(gps_instance)) {
+        AP::gps().handle_external(gps, gps_instance);
     }
 }
 
@@ -215,26 +219,26 @@ const char* AP_ExternalAHRS_MicroStrain5::get_name() const
 bool AP_ExternalAHRS_MicroStrain5::healthy(void) const
 {
     uint32_t now = AP_HAL::millis();
-    return (now - last_ins_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500);
+    return (now - last_imu_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500);
 }
 
 bool AP_ExternalAHRS_MicroStrain5::initialised(void) const
 {
-    return last_ins_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
+    return last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
 }
 
 bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
 {
     if (!healthy()) {
-        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain unhealthy");
+        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 unhealthy");
         return false;
     }
-    if (gnss_data.fix_type < 3) {
-        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock");
+    if (gnss_data[gnss_instance].fix_type < 3) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 no GPS lock");
         return false;
     }
     if (filter_status.state != 0x02) {
-        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running");
+        hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 filter not running");
         return false;
     }
 
@@ -244,15 +248,15 @@ bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t fail
 void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) const
 {
     memset(&status, 0, sizeof(status));
-    if (last_ins_pkt != 0 && last_gps_pkt != 0) {
+    if (last_imu_pkt != 0 && last_gps_pkt != 0) {
         status.flags.initalized = true;
     }
-    if (healthy() && last_ins_pkt != 0) {
+    if (healthy() && last_imu_pkt != 0) {
         status.flags.attitude = true;
         status.flags.vert_vel = true;
         status.flags.vert_pos = true;
 
-        if (gnss_data.fix_type >= 3) {
+        if (gnss_data[gnss_instance].fix_type >= 3) {
             status.flags.horiz_vel = true;
             status.flags.horiz_pos_rel = true;
             status.flags.horiz_pos_abs = true;
@@ -309,7 +313,7 @@ void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const
     const float hgt_gate = 4; // represents hz value data is posted at
     const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
     mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
-                                       gnss_data.speed_accuracy/vel_gate, gnss_data.horizontal_position_accuracy/pos_gate, gnss_data.vertical_position_accuracy/hgt_gate,
+                                       gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate,
                                        mag_var, 0, 0);
 
 }