From 17fc58f3cddc9da24c4dc5e184d0c8f319e84fd9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 14:11:33 +1100 Subject: [PATCH] AP_InertialSensor: re-work for more flexible main loop rates --- .../AP_InertialSensor/AP_InertialSensor.cpp | 18 ++---------------- .../AP_InertialSensor/AP_InertialSensor.h | 14 +++----------- 2 files changed, 5 insertions(+), 27 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 88b009e719..38c21435a8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -430,7 +430,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, } void -AP_InertialSensor::init(Sample_rate sample_rate) +AP_InertialSensor::init(uint16_t sample_rate) { // remember the sample rate _sample_rate = sample_rate; @@ -453,21 +453,7 @@ AP_InertialSensor::init(Sample_rate sample_rate) _init_gyro(); } - switch (sample_rate) { - case RATE_50HZ: - _sample_period_usec = 20000; - break; - case RATE_100HZ: - _sample_period_usec = 10000; - break; - case RATE_200HZ: - _sample_period_usec = 5000; - break; - case RATE_400HZ: - default: - _sample_period_usec = 2500; - break; - } + _sample_period_usec = 1000*1000UL / _sample_rate; // establish the baseline time between samples _delta_time = 0; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 3f433af74c..32d0c87495 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -49,14 +49,6 @@ public: AP_InertialSensor(); static AP_InertialSensor *get_instance(); - // the rate that updates will be available to the application - enum Sample_rate { - RATE_50HZ = 50, - RATE_100HZ = 100, - RATE_200HZ = 200, - RATE_400HZ = 400 - }; - enum Gyro_Calibration_Timing { GYRO_CAL_NEVER = 0, GYRO_CAL_STARTUP_ONLY = 1 @@ -70,7 +62,7 @@ public: /// /// @param style The initialisation startup style. /// - void init(Sample_rate sample_rate); + void init(uint16_t sample_rate_hz); /// Register a new gyro/accel driver, allocating an instance /// number @@ -179,7 +171,7 @@ public: } // return the selected sample rate - Sample_rate get_sample_rate(void) const { return _sample_rate; } + uint16_t get_sample_rate(void) const { return _sample_rate; } // return the main loop delta_t in seconds float get_loop_delta_t(void) const { return _loop_delta_t; } @@ -275,7 +267,7 @@ private: uint8_t _backend_count; // the selected sample rate - Sample_rate _sample_rate; + uint16_t _sample_rate; float _loop_delta_t; // Most recent accelerometer reading