Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: converted flymaple driver to new API

This commit is contained in:
Andrew Tridgell 2014-10-16 11:24:32 +11:00
parent a42af0f2f1
commit ff5f791343
4 changed files with 103 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -316,6 +316,8 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
_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);
#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
_backends[0] = AP_InertialSensor_Flymaple::detect(*this, sample_rate);
#else
#error Unrecognised HAL_INS_TYPE setting
#endif

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -277,6 +277,7 @@ private:
#include "AP_InertialSensor_Oilpan.h"
#include "AP_InertialSensor_MPU9250.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_Flymaple.h"
#include "AP_InertialSensor_HIL.h"
#include "AP_InertialSensor_UserInteract_Stream.h"
#include "AP_InertialSensor_UserInteract_MAVLink.h"

207
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -14,7 +14,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Flymaple port by Mike McCauley
Flymaple IMU driver by Mike McCauley
*/
// Interface to the Flymaple sensors:
@ -28,20 +28,6 @@
const extern AP_HAL::HAL& hal;
/// Statics
Vector3f AP_InertialSensor_Flymaple::_accel_filtered;
uint32_t AP_InertialSensor_Flymaple::_accel_samples;
Vector3f AP_InertialSensor_Flymaple::_gyro_filtered;
uint32_t AP_InertialSensor_Flymaple::_gyro_samples;
uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10);
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(800, 10);
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(800, 10);
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(800, 10);
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(800, 10);
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(800, 10);
// This is how often we wish to make raw samples of the sensors in Hz
const uint32_t raw_sample_rate_hz = 800;
// And the equivalent time between samples in microseconds
@ -77,24 +63,56 @@ const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz);
// Result wil be radians/sec
#define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f)
uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_gyro_sample(false),
_have_accel_sample(false),
_accel_filter_x(raw_sample_rate_hz, 10),
_accel_filter_y(raw_sample_rate_hz, 10),
_accel_filter_z(raw_sample_rate_hz, 10),
_gyro_filter_x(raw_sample_rate_hz, 10),
_gyro_filter_y(raw_sample_rate_hz, 10),
_gyro_filter_z(raw_sample_rate_hz, 10),
_last_gyro_timestamp(0),
_last_accel_timestamp(0)
{}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
{
AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
{
// Sensors are raw sampled at 800Hz.
// Here we figure the divider to get the rate that update should be called
switch (sample_rate) {
case RATE_50HZ:
_sample_divider = raw_sample_rate_hz / 50;
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 10;
break;
case RATE_100HZ:
_sample_divider = raw_sample_rate_hz / 100;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 20;
break;
case RATE_200HZ:
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
break;
default:
_sample_divider = raw_sample_rate_hz / 200;
_default_filter_hz = 20;
break;
return false;
}
// get pointer to i2c bus semaphore
@ -146,12 +164,15 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
hal.scheduler->delay(1);
// Set up the filter desired
_set_filter_frequency(_mpu6000_filter);
_set_filter_frequency(_imu.get_filter());
// give back i2c semaphore
// give back i2c semaphore
i2c_sem->give();
return AP_PRODUCT_ID_FLYMAPLE;
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
return true;
}
/*
@ -170,109 +191,47 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
_gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
if (!wait_for_sample(100)) {
return false;
}
Vector3f accel_scale = _accel_scale[0].get();
Vector3f accel, gyro;
uint32_t now = hal.scheduler->micros();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs();
// base the time on the gyro timestamp, as that is what is
// multiplied by time to integrate in DCM
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_previous_accel[0] = _accel[0];
_accel[0] = _accel_filtered;
_accel_samples = 0;
_gyro[0] = _gyro_filtered;
_gyro_samples = 0;
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_gyro_sample = false;
_have_accel_sample = false;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro[0].rotate(_board_orientation);
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel, now);
// Adjust for chip scaling to get radians/sec
_gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0];
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true;
}
bool AP_InertialSensor_Flymaple::get_gyro_health(void) const
{
if (_last_gyro_timestamp == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t now = hal.scheduler->micros();
if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) {
// gyros have not updated
return false;
}
return true;
}
bool AP_InertialSensor_Flymaple::get_accel_health(void) const
{
if (_last_accel_timestamp == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t now = hal.scheduler->micros();
if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) {
// gyros have not updated
return false;
}
return true;
}
float AP_InertialSensor_Flymaple::get_delta_time(void) const
{
return _delta_time;
}
float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void)
{
// Dont really know this for the ITG-3200.
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
// This needs to get called as often as possible.
// Its job is to accumulate samples as fast as is reasonable for the accel and gyro
// sensors.
// Cant call this from within the system timers, since the long I2C reads (up to 1ms)
// with interrupts disabled breaks lots of things
// Therefore must call this as often as possible from
// within the mainline and thropttle the reads here to suit the sensors
// Note that this is called from gyro_sample_available() and
// accel_sample_available(), which is really not good enough for
// 800Hz, as it is common for the main loop to take more than 1.5ms
// before wait_for_sample() is called again. We can't just call this
// from a timer as timers run with interrupts disabled, and the I2C
// operations take too long
// So we are stuck with a suboptimal solution. The results are not so
// good in terms of timing. It may be better with the FIFOs enabled
void AP_InertialSensor_Flymaple::_accumulate(void)
{
// get pointer to i2c bus semaphore
@ -285,7 +244,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
// Read accelerometer
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
uint8_t buffer[6];
uint64_t now = hal.scheduler->micros();
uint32_t now = hal.scheduler->micros();
// This takes about 250us at 400kHz I2C speed
if ((now - _last_accel_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
@ -300,7 +259,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
_accel_filtered = Vector3f(_accel_filter_x.apply(x),
_accel_filter_y.apply(y),
_accel_filter_z.apply(z));
_accel_samples++;
_have_accel_sample = true;
_last_accel_timestamp = now;
}
@ -317,7 +276,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
_gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
_gyro_filter_y.apply(y),
_gyro_filter_z.apply(z));
_gyro_samples++;
_have_gyro_sample = true;
_last_gyro_timestamp = now;
}
@ -325,26 +284,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
i2c_sem->give();
}
bool AP_InertialSensor_Flymaple::_sample_available(void)
{
_accumulate();
return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
}
bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100);
if (_sample_available()) {
return true;
}
}
return false;
}
#endif // CONFIG_HAL_BOARD

57
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -6,39 +6,32 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
#include <Filter.h>
#include <LowPassFilter2p.h>
class AP_InertialSensor_Flymaple : public AP_InertialSensor
class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_Flymaple(AP_InertialSensor &imu);
AP_InertialSensor_Flymaple() : AP_InertialSensor() {}
/* update accel and gyro state */
bool update();
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
bool wait_for_sample(uint16_t timeout_ms);
bool get_gyro_health(void) const;
bool get_accel_health(void) const;
bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; }
bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
private:
uint16_t _init_sensor( Sample_rate sample_rate );
static void _accumulate(void);
bool _sample_available();
uint64_t _last_update_usec;
float _delta_time;
static Vector3f _accel_filtered;
static uint32_t _accel_samples;
static Vector3f _gyro_filtered;
static uint32_t _gyro_samples;
static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider;
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
bool _have_gyro_sample;
bool _have_accel_sample;
// support for updating filter at runtime
uint8_t _last_filter_hz;
@ -46,12 +39,18 @@ private:
void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
static LowPassFilter2p _accel_filter_x;
static LowPassFilter2p _accel_filter_y;
static LowPassFilter2p _accel_filter_z;
static LowPassFilter2p _gyro_filter_x;
static LowPassFilter2p _gyro_filter_y;
static LowPassFilter2p _gyro_filter_z;
LowPassFilter2p _accel_filter_x;
LowPassFilter2p _accel_filter_y;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
uint8_t _gyro_instance;
uint8_t _accel_instance;
uint32_t _last_gyro_timestamp;
uint32_t _last_accel_timestamp;
};
#endif
#endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__