AP_Module: also allow export of raw gyro and accel samples

allows for oversampling in image correction

libraries/AP_Module/AP_Module.cpp

@@ -32,6 +32,8 @@ const char *AP_Module::hook_names[AP_Module::NUM_HOOKS] = {
     "hook_setup_start",
     "hook_setup_complete",
     "hook_AHRS_update",
+    "hook_gyro_sample",
+    "hook_accel_sample",
 };
 
 /*
@@ -192,3 +194,60 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
         fn(&state);
     }    
 }
+
+
+/*
+  call any gyro_sample hooks
+*/
+void AP_Module::call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f &gyro)
+{
+    if (hooks[HOOK_GYRO_SAMPLE] == nullptr) {
+        // avoid filling in struct
+        return;
+    }
+
+    /*
+      construct gyro_sample structure
+     */
+    struct gyro_sample state {};
+    state.structure_version = gyro_sample_version;
+    state.time_us = AP_HAL::micros64();
+    state.instance = instance;
+    state.delta_time = dt;
+    state.gyro[0] = gyro[0];
+    state.gyro[1] = gyro[1];
+    state.gyro[2] = gyro[2];
+
+    for (const struct hook_list *h=hooks[HOOK_GYRO_SAMPLE]; h; h=h->next) {
+        hook_gyro_sample_fn_t fn = reinterpret_cast<hook_gyro_sample_fn_t>(h->symbol);
+        fn(&state);
+    }    
+}
+
+/*
+  call any accel_sample hooks
+*/
+void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel)
+{
+    if (hooks[HOOK_ACCEL_SAMPLE] == nullptr) {
+        // avoid filling in struct
+        return;
+    }
+
+    /*
+      construct accel_sample structure
+     */
+    struct accel_sample state {};
+    state.structure_version = accel_sample_version;
+    state.time_us = AP_HAL::micros64();
+    state.instance = instance;
+    state.delta_time = dt;
+    state.accel[0] = accel[0];
+    state.accel[1] = accel[1];
+    state.accel[2] = accel[2];
+
+    for (const struct hook_list *h=hooks[HOOK_ACCEL_SAMPLE]; h; h=h->next) {
+        hook_accel_sample_fn_t fn = reinterpret_cast<hook_accel_sample_fn_t>(h->symbol);
+        fn(&state);
+    }    
+}

libraries/AP_Module/AP_Module.h

@@ -53,6 +53,12 @@ public:
     // call any AHRS_update hooks
     static void call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs);
 
+    // call any gyro_sample hooks
+    static void call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f &gyro);
+
+    // call any accel_sample hooks
+    static void call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel);
+    
     
 private:
 
@@ -60,6 +66,8 @@ private:
         HOOK_SETUP_START    = 0,
         HOOK_SETUP_COMPLETE,
         HOOK_AHRS_UPDATE,
+        HOOK_GYRO_SAMPLE,
+        HOOK_ACCEL_SAMPLE,
         NUM_HOOKS
     };
 

libraries/AP_Module/AP_Module_Structures.h

@@ -16,6 +16,8 @@ extern "C" {
 #include <stdbool.h>
 
 #define AHRS_state_version 1
+#define gyro_sample_version 1
+#define accel_sample_version 1
 
 enum AHRS_status {
     AHRS_STATUS_INITIALISING    = 0,
@@ -82,6 +84,46 @@ struct AHRS_state {
 };
 
 
+/*
+  export corrected gyro samples at hardware sampling rate
+ */
+struct gyro_sample {
+    // version of this structure (gyro_sample_version)
+    uint32_t structure_version;
+
+    // which gyro this is
+    uint8_t instance;
+    
+    // time since boot in microseconds
+    uint64_t time_us;
+
+    // time associated with this sample (seconds)
+    float delta_time;
+    
+    // body frame rates in radian/sec
+    float gyro[3];
+};
+
+/*
+  export corrected accel samples at hardware sampling rate
+ */
+struct accel_sample {
+    // version of this structure (accel_sample_version)
+    uint32_t structure_version;
+
+    // which accel this is
+    uint8_t instance;
+    
+    // time since boot in microseconds
+    uint64_t time_us;
+
+    // time associated with this sample (seconds)
+    float delta_time;
+
+    // body frame rates in m/s/s
+    float accel[3];
+};
+    
 /*
   prototypes for hook functions
  */
@@ -94,6 +136,12 @@ void hook_setup_complete(uint64_t time_us);
 typedef void (*hook_AHRS_update_fn_t)(const struct AHRS_state *);
 void hook_AHRS_update(const struct AHRS_state *state);
 
+typedef void (*hook_gyro_sample_fn_t)(const struct gyro_sample *);
+void hook_gyro_sample(const struct gyro_sample *state);
+
+typedef void (*hook_accel_sample_fn_t)(const struct accel_sample *);
+void hook_accel_sample(const struct accel_sample *state);
+    
 #ifdef __cplusplus
 }
 #endif