mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: improved timing in all drivers
This commit is contained in:
parent
ff6d87f145
commit
deafcd6ddc
|
@ -221,10 +221,11 @@ AP_InertialSensor::AP_InertialSensor() :
|
||||||
_gyro(),
|
_gyro(),
|
||||||
_board_orientation(ROTATION_NONE),
|
_board_orientation(ROTATION_NONE),
|
||||||
_gyro_count(0),
|
_gyro_count(0),
|
||||||
_accel_count(0)
|
_accel_count(0),
|
||||||
|
_hil_mode(false)
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
|
||||||
_backends[i] = NULL;
|
_backends[i] = NULL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -291,7 +292,11 @@ AP_InertialSensor::init( Start_style style,
|
||||||
_sample_period_usec = 2500;
|
_sample_period_usec = 2500;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
_delta_time = 1.0e-6f * _sample_period_usec;
|
|
||||||
|
// establish the baseline time between samples
|
||||||
|
_base_delta_time = 1.0e-6f * _sample_period_usec;
|
||||||
|
_delta_time = 0;
|
||||||
|
_have_sample = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -307,6 +312,10 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
|
||||||
_backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate);
|
_backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate);
|
||||||
#elif HAL_INS_DEFAULT == HAL_INS_PX4
|
#elif HAL_INS_DEFAULT == HAL_INS_PX4
|
||||||
_backends[0] = AP_InertialSensor_PX4::detect(*this, sample_rate);
|
_backends[0] = AP_InertialSensor_PX4::detect(*this, sample_rate);
|
||||||
|
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
|
||||||
|
_backends[0] = AP_InertialSensor_Oilpan::detect(*this, sample_rate);
|
||||||
|
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250
|
||||||
|
_backends[0] = AP_InertialSensor_MPU9250::detect(*this, sample_rate);
|
||||||
#else
|
#else
|
||||||
#error Unrecognised HAL_INS_TYPE setting
|
#error Unrecognised HAL_INS_TYPE setting
|
||||||
#endif
|
#endif
|
||||||
|
@ -907,51 +916,110 @@ void AP_InertialSensor::_save_parameters()
|
||||||
*/
|
*/
|
||||||
void AP_InertialSensor::update(void)
|
void AP_InertialSensor::update(void)
|
||||||
{
|
{
|
||||||
for (int8_t i=INS_MAX_INSTANCES-1; i>=0; i--) {
|
// during initialisation update() may be called without
|
||||||
if (_backends[i] != NULL) {
|
// wait_for_sample(), and a wait is implied
|
||||||
_backends[i]->update();
|
wait_for_sample();
|
||||||
}
|
|
||||||
}
|
if (!_hil_mode) {
|
||||||
|
for (int8_t i=0; i<INS_MAX_BACKENDS; i++) {
|
||||||
|
if (_backends[i] != NULL) {
|
||||||
|
_backends[i]->update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_have_sample = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
wait for a sample to be available. This waits for at least one new
|
wait for a sample to be available. This is the function that
|
||||||
accel and one new gyro sample. It is up to the backend to define
|
determines the timing of the main loop in ardupilot.
|
||||||
what a new sample means. Some backends are based on the sensor
|
|
||||||
providing a sample, some are based on time.
|
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)
|
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();
|
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) {
|
while (now - _last_sample_usec > _sample_period_usec) {
|
||||||
_last_sample_usec += _sample_period_usec;
|
_last_sample_usec += _sample_period_usec;
|
||||||
have_sample = true;
|
sample_count++;
|
||||||
}
|
if (sample_count == 1000) {
|
||||||
if (!have_sample) {
|
// we've gone a very long time between samples, and gyro
|
||||||
uint32_t sample_due = _last_sample_usec + _sample_period_usec;
|
// integration times beyond this don't really make
|
||||||
uint32_t wait_usec = sample_due - now;
|
// sense. Just reset the timing
|
||||||
hal.scheduler->delay_microseconds(wait_usec);
|
_last_sample_usec = now;
|
||||||
_last_sample_usec += _sample_period_usec;
|
_delta_time = sample_count * _base_delta_time;
|
||||||
|
goto check_sample;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// but also wait for at least one backend to have a sample of both
|
if (sample_count == 0) {
|
||||||
// accel and gyro
|
// this is the normal case, and it means we took less than
|
||||||
bool gyro_available = false;
|
// _sample_period_usec to process the last sample. We're
|
||||||
bool accel_available = false;
|
// on-time. So we can just sleep for the right number of
|
||||||
while (!gyro_available || !accel_available) {
|
// microseconds to get us to the next sample.
|
||||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
_last_sample_usec += _sample_period_usec;
|
||||||
if (_backends[i] != NULL) {
|
uint32_t wait_usec = _last_sample_usec - now;
|
||||||
gyro_available |= _backends[i]->gyro_sample_available();
|
if (wait_usec > 100 && wait_usec <= _sample_period_usec) {
|
||||||
accel_available |= _backends[i]->accel_sample_available();
|
// 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; i<INS_MAX_BACKENDS; i++) {
|
||||||
|
if (_backends[i] != NULL) {
|
||||||
|
gyro_available |= _backends[i]->gyro_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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -13,10 +13,13 @@
|
||||||
*/
|
*/
|
||||||
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
|
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
|
||||||
#define INS_MAX_INSTANCES 3
|
#define INS_MAX_INSTANCES 3
|
||||||
|
#define INS_MAX_BACKENDS 6
|
||||||
#else
|
#else
|
||||||
#define INS_MAX_INSTANCES 1
|
#define INS_MAX_INSTANCES 1
|
||||||
|
#define INS_MAX_BACKENDS 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <AP_HAL.h>
|
#include <AP_HAL.h>
|
||||||
#include <AP_Math.h>
|
#include <AP_Math.h>
|
||||||
|
@ -180,6 +183,9 @@ public:
|
||||||
|
|
||||||
uint8_t get_primary_accel(void) const { return 0; }
|
uint8_t get_primary_accel(void) const { return 0; }
|
||||||
|
|
||||||
|
// enable HIL mode
|
||||||
|
void set_hil_mode(void) { _hil_mode = true; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// load backend drivers
|
// load backend drivers
|
||||||
|
@ -206,7 +212,7 @@ private:
|
||||||
void _save_parameters();
|
void _save_parameters();
|
||||||
|
|
||||||
// backend objects
|
// 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
|
// number of gyros and accel drivers. Note that most backends
|
||||||
// provide both accel and gyro data, so will increment both
|
// provide both accel and gyro data, so will increment both
|
||||||
|
@ -245,7 +251,17 @@ private:
|
||||||
uint8_t _primary_gyro;
|
uint8_t _primary_gyro;
|
||||||
uint8_t _primary_accel;
|
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;
|
float _delta_time;
|
||||||
|
|
||||||
// last time a wait_for_sample() returned a sample
|
// last time a wait_for_sample() returned a sample
|
||||||
|
@ -258,6 +274,8 @@ private:
|
||||||
#include "AP_InertialSensor_Backend.h"
|
#include "AP_InertialSensor_Backend.h"
|
||||||
#include "AP_InertialSensor_MPU6000.h"
|
#include "AP_InertialSensor_MPU6000.h"
|
||||||
#include "AP_InertialSensor_PX4.h"
|
#include "AP_InertialSensor_PX4.h"
|
||||||
|
#include "AP_InertialSensor_Oilpan.h"
|
||||||
|
#include "AP_InertialSensor_MPU9250.h"
|
||||||
#include "AP_InertialSensor_HIL.h"
|
#include "AP_InertialSensor_HIL.h"
|
||||||
#include "AP_InertialSensor_UserInteract_Stream.h"
|
#include "AP_InertialSensor_UserInteract_Stream.h"
|
||||||
#include "AP_InertialSensor_UserInteract_MAVLink.h"
|
#include "AP_InertialSensor_UserInteract_MAVLink.h"
|
||||||
|
|
|
@ -31,7 +31,8 @@ public:
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Update the sensor data. Called by the frontend to transfer
|
* 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;
|
virtual bool update() = 0;
|
||||||
|
|
||||||
|
@ -48,7 +49,8 @@ public:
|
||||||
virtual bool gyro_sample_available() = 0;
|
virtual bool gyro_sample_available() = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
AP_InertialSensor &_imu; ///< access to frontend
|
// access to frontend
|
||||||
|
AP_InertialSensor &_imu;
|
||||||
|
|
||||||
// rotate gyro vector and offset
|
// rotate gyro vector and offset
|
||||||
void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now);
|
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
|
// rotate accel vector, scale and offset
|
||||||
void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
|
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
|
// function which instantiates an instance of the backend sensor
|
||||||
// driver.
|
// driver if the sensor is available
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_INERTIALSENSOR_BACKEND_H__
|
#endif // __AP_INERTIALSENSOR_BACKEND_H__
|
||||||
|
|
Loading…
Reference in New Issue