From deafcd6ddc38fc5525f9bc8800ed7ade4770638f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Oct 2014 10:32:40 +1100 Subject: [PATCH] AP_InertialSensor: improved timing in all drivers --- .../AP_InertialSensor/AP_InertialSensor.cpp | 134 +++++++++++++----- .../AP_InertialSensor/AP_InertialSensor.h | 22 ++- .../AP_InertialSensor_Backend.h | 10 +- 3 files changed, 127 insertions(+), 39 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 5889aa78f0..41e4bb4ef5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -221,10 +221,11 @@ AP_InertialSensor::AP_InertialSensor() : _gyro(), _board_orientation(ROTATION_NONE), _gyro_count(0), - _accel_count(0) + _accel_count(0), + _hil_mode(false) { AP_Param::setup_object_defaults(this, var_info); - for (uint8_t i=0; i=0; i--) { - if (_backends[i] != NULL) { - _backends[i]->update(); - } - } + // during initialisation update() may be called without + // wait_for_sample(), and a wait is implied + wait_for_sample(); + + if (!_hil_mode) { + for (int8_t i=0; iupdate(); + } + } + } + + _have_sample = false; } /* - wait for a sample to be available. This waits for at least one new - accel and one new gyro sample. It is up to the backend to define - what a new sample means. Some backends are based on the sensor - providing a sample, some are based on time. + wait for a sample to be available. This is the function that + determines the timing of the main loop in ardupilot. + + Ideally this function would return at exactly the rate given by the + sample_rate argument given to AP_InertialSensor::init(). + + The key output of this function is _delta_time, which is the time + over which the gyro and accel integration will happen for this + sample. We want that to be a constant time if possible, but if + delays occur we need to cope with them. The long term sum of + _delta_time should be exactly equal to the wall clock elapsed time */ void AP_InertialSensor::wait_for_sample(void) { - bool have_sample = false; + if (_have_sample) { + // the user has called wait_for_sample() again without + // consuming the sample with update() + return; + } - // wait the right amount of time for a sample to be due uint32_t now = hal.scheduler->micros(); + + if (_last_sample_usec == 0 && _delta_time <= 0) { + // this is the first call to wait_for_sample() + _last_sample_usec = now; + _delta_time = _base_delta_time; + } + + // see how many samples worth of time have elapsed + uint16_t sample_count = 0; while (now - _last_sample_usec > _sample_period_usec) { _last_sample_usec += _sample_period_usec; - have_sample = true; - } - if (!have_sample) { - uint32_t sample_due = _last_sample_usec + _sample_period_usec; - uint32_t wait_usec = sample_due - now; - hal.scheduler->delay_microseconds(wait_usec); - _last_sample_usec += _sample_period_usec; + sample_count++; + if (sample_count == 1000) { + // we've gone a very long time between samples, and gyro + // integration times beyond this don't really make + // sense. Just reset the timing + _last_sample_usec = now; + _delta_time = sample_count * _base_delta_time; + goto check_sample; + } } - // but also wait for at least one backend to have a sample of both - // accel and gyro - bool gyro_available = false; - bool accel_available = false; - while (!gyro_available || !accel_available) { - for (uint8_t i=0; igyro_sample_available(); - accel_available |= _backends[i]->accel_sample_available(); + if (sample_count == 0) { + // this is the normal case, and it means we took less than + // _sample_period_usec to process the last sample. We're + // on-time. So we can just sleep for the right number of + // microseconds to get us to the next sample. + _last_sample_usec += _sample_period_usec; + uint32_t wait_usec = _last_sample_usec - now; + if (wait_usec > 100 && wait_usec <= _sample_period_usec) { + // only delay if the time to wait is significant. + hal.scheduler->delay_microseconds(wait_usec); + } + + // we set the _delta_time to the base time, ignoring small + // timing fluctuations. + _delta_time = _base_delta_time; + } else { + // we have had some sort of delay and have missed our time + // slot. We want to run this sample immediately, and need to + // adjust _delta_time to reflect the delay we've had + _delta_time = _base_delta_time*sample_count + 1.0e-6f*(now - _last_sample_usec); + + // reset the time base to the current time + _last_sample_usec = now; + } + +check_sample: + if (!_hil_mode) { + // but also wait for at least one backend to have a sample of both + // accel and gyro. This normally completes immediately. + bool gyro_available = false; + bool accel_available = false; + while (!gyro_available || !accel_available) { + for (uint8_t i=0; igyro_sample_available(); + accel_available |= _backends[i]->accel_sample_available(); + } + } + if (!gyro_available || !accel_available) { + hal.scheduler->delay_microseconds(100); } } - if (!gyro_available || !accel_available) { - hal.scheduler->delay_microseconds(100); - } } + + _have_sample = true; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index bf1099ffb6..ce86705767 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -13,10 +13,13 @@ */ #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 #define INS_MAX_INSTANCES 3 +#define INS_MAX_BACKENDS 6 #else #define INS_MAX_INSTANCES 1 +#define INS_MAX_BACKENDS 1 #endif + #include #include #include @@ -180,6 +183,9 @@ public: uint8_t get_primary_accel(void) const { return 0; } + // enable HIL mode + void set_hil_mode(void) { _hil_mode = true; } + private: // load backend drivers @@ -206,7 +212,7 @@ private: void _save_parameters(); // backend objects - AP_InertialSensor_Backend *_backends[INS_MAX_INSTANCES]; + AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS]; // number of gyros and accel drivers. Note that most backends // provide both accel and gyro data, so will increment both @@ -245,7 +251,17 @@ private: uint8_t _primary_gyro; uint8_t _primary_accel; - // time between samples + // has wait_for_sample() found a sample? + bool _have_sample:1; + + // are we in HIL mode? + bool _hil_mode:1; + + // the delta time in seconds between samples when there are no + // delays + float _base_delta_time; + + // the delta time in seconds for the last sample float _delta_time; // last time a wait_for_sample() returned a sample @@ -258,6 +274,8 @@ private: #include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_PX4.h" +#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index d6b373b5c0..44da6dc502 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -31,7 +31,8 @@ public: /* * Update the sensor data. Called by the frontend to transfer - * accumulated sensor readings to the frontend structure + * accumulated sensor readings to the frontend structure via the + * _rotate_and_offset_gyro() and _rotate_and_offset_accel() functions */ virtual bool update() = 0; @@ -48,7 +49,8 @@ public: virtual bool gyro_sample_available() = 0; protected: - AP_InertialSensor &_imu; ///< access to frontend + // access to frontend + AP_InertialSensor &_imu; // rotate gyro vector and offset void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now); @@ -56,9 +58,9 @@ protected: // rotate accel vector, scale and offset void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now); - // note that each backend is also expected to have a detect() + // note that each backend is also expected to have a static detect() // function which instantiates an instance of the backend sensor - // driver. + // driver if the sensor is available }; #endif // __AP_INERTIALSENSOR_BACKEND_H__