AP_Module: added gyro_bias to AHRS structure

libraries/AP_Module/AP_Module.cpp

@@ -193,6 +193,14 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
     state.accel_ef[0] = accel_ef[0];
     state.accel_ef[1] = accel_ef[0];
     state.accel_ef[2] = accel_ef[0];
+
+    state.primary_accel = ahrs.get_primary_accel_index();
+    state.primary_gyro = ahrs.get_primary_gyro_index();
+
+    const Vector3f &gyro_bias = ahrs.get_gyro_drift();
+    state.gyro_bias[0] = gyro_bias[0];
+    state.gyro_bias[1] = gyro_bias[1];
+    state.gyro_bias[2] = gyro_bias[2];
     
     for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) {
         ap_hook_AHRS_update_fn_t fn = reinterpret_cast<ap_hook_AHRS_update_fn_t>(h->symbol);

libraries/AP_Module/AP_Module_Structures.h

@@ -15,7 +15,7 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 
-#define AHRS_state_version 1
+#define AHRS_state_version 2
 #define gyro_sample_version 1
 #define accel_sample_version 2
 
@@ -81,6 +81,17 @@ struct AHRS_state {
     // current earth frame acceleration estimate, including
     // gravitational forces, m/s/s order is NED
     float accel_ef[3];
+
+    // the current primary accel instance
+    uint8_t primary_accel;
+
+    // the current primary gyro instance
+    uint8_t primary_gyro;
+    
+    // current gyro bias. This is relative to the gyro data in
+    // gyro_sample for primary_gyro. It should be added to a gyro
+    // sample to get the corrected gyro estimate
+    float gyro_bias[3];
 };