ExternalAHRS: fixed IMU access, check origin and common logging

libraries/AP_AHRS/AP_AHRS_External.cpp

@@ -23,17 +23,25 @@ bool AP_AHRS_External::healthy() const {
 void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
 {
     Quaternion quat;
-    if (!AP::externalAHRS().get_quaternion(quat)) {
+    auto &extahrs = AP::externalAHRS();
+    const AP_InertialSensor &_ins = AP::ins();
+    if (!extahrs.get_quaternion(quat)) {
         return;
     }
     quat.rotation_matrix(results.dcm_matrix);
     results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
     results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);
 
     results.gyro_drift.zero();
-    results.gyro_estimate = AP::externalAHRS().get_gyro();
+    if (!extahrs.get_gyro(results.gyro_estimate)) {
+        results.gyro_estimate = _ins.get_gyro();
+    }
+
+    Vector3f accel;
+    if (!extahrs.get_accel(accel)) {
+        accel = _ins.get_accel();
+    }
 
-    const Vector3f accel = AP::externalAHRS().get_accel();
     const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     results.accel_ef = accel_ef;
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -29,6 +29,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -80,6 +81,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass
     // @User: Advanced
     AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF),
+
+    // @Param: _LOG_RATE
+    // @DisplayName: AHRS logging rate
+    // @Description: Logging rate for EARHS devices
+    // @Units: Hz
+    // @User: Standard
+    AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
 
     AP_GROUPEND
 };
@@ -228,6 +236,10 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
         return false;
     }
 
+    if (!state.have_origin) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
+        return false;
+    }
     return backend->pre_arm_check(failure_msg, failure_msg_len);
 }
 
@@ -242,16 +254,24 @@ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
     }
 }
 
-Vector3f AP_ExternalAHRS::get_gyro(void)
+bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
 {
     WITH_SEMAPHORE(state.sem);
-    return state.gyro;
+    if (!has_sensor(AvailableSensor::IMU)) {
+        return false;
+    }
+    gyro = state.gyro;
+    return true;
 }
 
-Vector3f AP_ExternalAHRS::get_accel(void)
+bool AP_ExternalAHRS::get_accel(Vector3f &accel)
 {
     WITH_SEMAPHORE(state.sem);
-    return state.accel;
+    if (!has_sensor(AvailableSensor::IMU)) {
+        return false;
+    }
+    accel = state.accel;
+    return true;
 }
 
 // send an EKF_STATUS message to GCS
@@ -281,6 +301,39 @@ void AP_ExternalAHRS::update(void)
             state.have_origin = true;
         }
     }
+    const uint32_t now_ms = AP_HAL::millis();
+    if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {
+        last_log_ms = now_ms;
+
+        // @LoggerMessage: EAHR
+        // @Description: External AHRS data
+        // @Field: TimeUS: Time since system startup
+        // @Field: Roll: euler roll
+        // @Field: Pitch: euler pitch
+        // @Field: Yaw: euler yaw
+        // @Field: VN: velocity north
+        // @Field: VE: velocity east
+        // @Field: VD: velocity down
+        // @Field: Lat: latitude
+        // @Field: Lon: longitude
+        // @Field: Alt: altitude AMSL
+        // @Field: Flg: nav status flags
+
+        float roll, pitch, yaw;
+        state.quat.to_euler(roll, pitch, yaw);
+        nav_filter_status filterStatus {};
+        get_filter_status(filterStatus);
+
+        AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg",
+                                    "sdddnnnDUm-",
+                                    "F000000GG0-",
+                                    "QffffffLLfI",
+                                    AP_HAL::micros64(),
+                                    degrees(roll), degrees(pitch), degrees(yaw),
+                                    state.velocity.x, state.velocity.y, state.velocity.z,
+                                    state.location.lat, state.location.lng, state.location.alt*0.01,
+                                    filterStatus.value);
+    }
 }
 
 // Get model/type name

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -115,8 +115,8 @@ class AP_ExternalAHRS {
     bool get_speed_down(float &speedD);
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
     void get_filter_status(nav_filter_status &status) const;
-    Vector3f get_gyro(void);
-    Vector3f get_accel(void);
+    bool get_gyro(Vector3f &gyro);
+    bool get_accel(Vector3f &accel);
     void send_status_report(class GCS_MAVLINK &link) const;
 
     // update backend
@@ -163,7 +163,12 @@ class AP_ExternalAHRS {
         float differential_pressure; // Pa
         float temperature; // degC
     } airspeed_data_message_t;
-
+
+    // set GNSS disable for auxillary function GPS_DISABLE
+    void set_gnss_disable(bool disable) {
+        gnss_is_disabled = disable;
+    }
+
 protected:
 
     enum class OPTIONS {
@@ -176,6 +181,7 @@ class AP_ExternalAHRS {
 
     AP_Enum<DevType> devtype;
     AP_Int16         rate;
+    AP_Int16         log_rate;
     AP_Int16         options;
     AP_Int16         sensors;
 
@@ -190,6 +196,11 @@ class AP_ExternalAHRS {
     void set_default_sensors(uint16_t _sensors) {
         sensors.set_default(_sensors);
     }
+
+    uint32_t last_log_ms;
+
+    // true when user has disabled the GNSS
+    bool gnss_is_disabled;
 };
 
 namespace AP {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp

@@ -501,44 +501,22 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
         // @LoggerMessage: ILB1
         // @Description: InertialLabs AHRS data1
         // @Field: TimeUS: Time since system startup
-        // @Field: Roll: euler roll
-        // @Field: Pitch: euler pitch
-        // @Field: Yaw: euler yaw
-        // @Field: VN: velocity north
-        // @Field: VE: velocity east
-        // @Field: VD: velocity down
-        // @Field: Lat: latitude
-        // @Field: Lon: longitude
-        // @Field: Alt: altitude AMSL
-
-        AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt",
-                                    "sdddnnnDUm",
-                                    "F000000GG0",
-                                    "QffffffLLf",
-                                    now_us,
-                                    degrees(roll), degrees(pitch), degrees(yaw),
-                                    state.velocity.x, state.velocity.y, state.velocity.z,
-                                    state.location.lat, state.location.lng, state.location.alt*0.01);
-
-        // @LoggerMessage: ILB2
-        // @Description: InertialLabs AHRS data2
-        // @Field: TimeUS: Time since system startup
         // @Field: PosVarN: position variance north
         // @Field: PosVarE: position variance east
         // @Field: PosVarD: position variance down
         // @Field: VelVarN: velocity variance north
         // @Field: VelVarE: velocity variance east
         // @Field: VelVarD: velocity variance down
 
-        AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
+        AP::logger().WriteStreaming("ILB1", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
                                     "smmmnnn",
                                     "F000000",
                                     "Qffffff",
                                     now_us,
                                     state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z,
                                     state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z);
 
-        // @LoggerMessage: ILB3
+        // @LoggerMessage: ILB2
         // @Description: InertialLabs AHRS data3
         // @Field: TimeUS: Time since system startup
         // @Field: Stat1: unit status1
@@ -553,7 +531,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
         // @Field: WVE: Wind velocity east
         // @Field: WVD: Wind velocity down
 
-        AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
+        AP::logger().WriteStreaming("ILB2", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
                                     "s-----------",
                                     "F-----------",
                                     "QHHBBBBBffff",

libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp

@@ -511,38 +511,6 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b)
 
         AP::ins().handle_external(ins);
     }
-
-
-#if HAL_LOGGING_ENABLED
-    // @LoggerMessage: EAH1
-    // @Description: External AHRS data
-    // @Field: TimeUS: Time since system startup
-    // @Field: Roll: euler roll
-    // @Field: Pitch: euler pitch
-    // @Field: Yaw: euler yaw
-    // @Field: VN: velocity north
-    // @Field: VE: velocity east
-    // @Field: VD: velocity down
-    // @Field: Lat: latitude
-    // @Field: Lon: longitude
-    // @Field: Alt: altitude AMSL
-    // @Field: UXY: uncertainty in XY position
-    // @Field: UV: uncertainty in velocity
-    // @Field: UR: uncertainty in roll
-    // @Field: UP: uncertainty in pitch
-    // @Field: UY: uncertainty in yaw
-
-    AP::logger().WriteStreaming("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
-                       "sdddnnnDUmmnddd", "F000000GG000000",
-                       "QffffffLLffffff",
-                       AP_HAL::micros64(),
-                       pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
-                       pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
-                       int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
-                       float(pkt1.positionLLA[2]),
-                       pkt1.posU, pkt1.velU,
-                       pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]);
-#endif  // HAL_LOGGING_ENABLED
 }
 
 /*

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp

@@ -17,6 +17,7 @@
  */
 
 #include "AP_ExternalAHRS_backend.h"
+#include <AP_AHRS/AP_AHRS.h>
 
 #if HAL_EXTERNAL_AHRS_ENABLED
 
@@ -37,5 +38,10 @@ bool AP_ExternalAHRS_backend::option_is_set(AP_ExternalAHRS::OPTIONS option) con
     return frontend.option_is_set(option);
 }
 
+bool AP_ExternalAHRS_backend::in_fly_forward(void) const
+{
+    return AP::ahrs().get_fly_forward();
+}
+
 #endif  // HAL_EXTERNAL_AHRS_ENABLED
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -57,7 +57,19 @@ class AP_ExternalAHRS_backend {
     void set_default_sensors(uint16_t sensors) {
         frontend.set_default_sensors(sensors);
     }
-
+
+    /*
+      return true if the GNSS is disabled
+     */
+    bool gnss_is_disabled(void) const {
+        return frontend.gnss_is_disabled;
+    }
+
+    /*
+      return true when we are in fixed wing flight
+     */
+    bool in_fly_forward(void) const;
+
 private:
     AP_ExternalAHRS &frontend;
 };

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -2532,7 +2532,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
     }
 
     // remove existing accel offsets and scaling
-    for (uint8_t k=0; k<num_accels; k++) {
+    for (uint8_t k=0; k<INS_MAX_INSTANCES; k++) {
         _accel_offset(k).set(Vector3f());
         _accel_scale(k).set(Vector3f(1,1,1));
         new_accel_offset[k].zero();
@@ -2620,6 +2620,12 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
             caltemp_accel(k).set_and_save(get_temperature(k));
 #endif
         }
+        for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
+            _accel_offset(k).set_and_save(Vector3f());
+            _accel_scale(k).set_and_save(Vector3f());
+            _gyro_offset(k).set_and_save(Vector3f());
+            _accel_id(k).set_and_save(0);
+        }
 
 #if AP_AHRS_ENABLED
         // force trim to zero

libraries/RC_Channel/RC_Channel.cpp

@@ -1378,6 +1378,9 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch
 
     case AUX_FUNC::GPS_DISABLE:
         AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH);
+#if HAL_EXTERNAL_AHRS_ENABLED
+        AP::externalAHRS().set_gnss_disable(ch_flag == AuxSwitchPos::HIGH);
+#endif
         break;
 
     case AUX_FUNC::GPS_DISABLE_YAW: