From c0d21c57306db8bbb115e4941bbe61a1552f9731 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Sep 2016 18:00:04 +1000 Subject: [PATCH] AP_Module: added gyro_bias to AHRS structure --- libraries/AP_Module/AP_Module.cpp | 8 ++++++++ libraries/AP_Module/AP_Module_Structures.h | 13 ++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Module/AP_Module.cpp b/libraries/AP_Module/AP_Module.cpp index f11eb637a9..d387f778cc 100644 --- a/libraries/AP_Module/AP_Module.cpp +++ b/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(h->symbol); diff --git a/libraries/AP_Module/AP_Module_Structures.h b/libraries/AP_Module/AP_Module_Structures.h index 5e820c31d2..eff5b707cf 100644 --- a/libraries/AP_Module/AP_Module_Structures.h +++ b/libraries/AP_Module/AP_Module_Structures.h @@ -15,7 +15,7 @@ extern "C" { #include #include -#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]; };