diff --git a/libraries/AP_Module/AP_Module.cpp b/libraries/AP_Module/AP_Module.cpp index 741a83b56a..f11eb637a9 100644 --- a/libraries/AP_Module/AP_Module.cpp +++ b/libraries/AP_Module/AP_Module.cpp @@ -235,7 +235,7 @@ void AP_Module::call_hook_gyro_sample(uint8_t instance, float dt, const Vector3f /* call any accel_sample hooks */ -void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel) +void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel, bool fsync_set) { #if AP_MODULE_SUPPORTED if (hooks[HOOK_ACCEL_SAMPLE] == nullptr) { @@ -254,6 +254,7 @@ void AP_Module::call_hook_accel_sample(uint8_t instance, float dt, const Vector3 state.accel[0] = accel[0]; state.accel[1] = accel[1]; state.accel[2] = accel[2]; + state.fsync_set = fsync_set; for (const struct hook_list *h=hooks[HOOK_ACCEL_SAMPLE]; h; h=h->next) { ap_hook_accel_sample_fn_t fn = reinterpret_cast(h->symbol); diff --git a/libraries/AP_Module/AP_Module.h b/libraries/AP_Module/AP_Module.h index 8cf5fb73e3..429abd6b52 100644 --- a/libraries/AP_Module/AP_Module.h +++ b/libraries/AP_Module/AP_Module.h @@ -60,7 +60,7 @@ public: 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); + static void call_hook_accel_sample(uint8_t instance, float dt, const Vector3f &accel, bool fsync_set); private: diff --git a/libraries/AP_Module/AP_Module_Structures.h b/libraries/AP_Module/AP_Module_Structures.h index 8905a3af8f..5e820c31d2 100644 --- a/libraries/AP_Module/AP_Module_Structures.h +++ b/libraries/AP_Module/AP_Module_Structures.h @@ -17,7 +17,7 @@ extern "C" { #define AHRS_state_version 1 #define gyro_sample_version 1 -#define accel_sample_version 1 +#define accel_sample_version 2 enum AHRS_status { AHRS_STATUS_INITIALISING = 0, @@ -122,6 +122,9 @@ struct accel_sample { // body frame rates in m/s/s float accel[3]; + + // true if external frame sync is set + bool fsync_set; }; /* diff --git a/libraries/AP_Module/examples/ModuleTest/module/moduletest.c b/libraries/AP_Module/examples/ModuleTest/module/moduletest.c index ca89074266..4d5c1b62cc 100644 --- a/libraries/AP_Module/examples/ModuleTest/module/moduletest.c +++ b/libraries/AP_Module/examples/ModuleTest/module/moduletest.c @@ -53,13 +53,21 @@ void ap_hook_gyro_sample(const struct gyro_sample *state) void ap_hook_accel_sample(const struct accel_sample *state) { static uint64_t last_print_us; + static uint32_t counter; + static uint32_t fsync_count; + counter++; + if (state->fsync_set) { + fsync_count++; + } if (state->time_us - last_print_us < 1000000UL) { return; } last_print_us = state->time_us; // print accels once per second - printf("accel (%.1f,%.1f,%.1f)\n", + printf("accel (%.1f,%.1f,%.1f) %lu %lu\n", state->accel[0], state->accel[1], - state->accel[2]); + state->accel[2], + (unsigned long)counter, + (unsigned long)fsync_count); }