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); _backends[0] = AP_InertialSensor_Oilpan::detect(*this, sample_rate);
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250
_backends[0] = AP_InertialSensor_MPU9250::detect(*this, sample_rate); _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 #else
#error Unrecognised HAL_INS_TYPE setting #error Unrecognised HAL_INS_TYPE setting
#endif #endif

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -277,6 +277,7 @@ private:
#include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_Oilpan.h"
#include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_MPU9250.h"
#include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_Flymaple.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"

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/>. 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: // Interface to the Flymaple sensors:
@ -28,20 +28,6 @@
const extern AP_HAL::HAL& hal; 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 // This is how often we wish to make raw samples of the sensors in Hz
const uint32_t raw_sample_rate_hz = 800; const uint32_t raw_sample_rate_hz = 800;
// And the equivalent time between samples in microseconds // 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 // Result wil be radians/sec
#define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f) #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. // Sensors are raw sampled at 800Hz.
// Here we figure the divider to get the rate that update should be called // Here we figure the divider to get the rate that update should be called
switch (sample_rate) { switch (sample_rate) {
case RATE_50HZ: case AP_InertialSensor::RATE_50HZ:
_sample_divider = raw_sample_rate_hz / 50;
_default_filter_hz = 10; _default_filter_hz = 10;
break; break;
case RATE_100HZ: case AP_InertialSensor::RATE_100HZ:
_sample_divider = raw_sample_rate_hz / 100;
_default_filter_hz = 20; _default_filter_hz = 20;
break; 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: default:
_sample_divider = raw_sample_rate_hz / 200; return false;
_default_filter_hz = 20;
break;
} }
// get pointer to i2c bus semaphore // 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); hal.scheduler->delay(1);
// Set up the filter desired // 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(); 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); _gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// This takes about 20us to run // This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void) bool AP_InertialSensor_Flymaple::update(void)
{ {
if (!wait_for_sample(100)) { Vector3f accel, gyro;
return false; uint32_t now = hal.scheduler->micros();
}
Vector3f accel_scale = _accel_scale[0].get();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
// base the time on the gyro timestamp, as that is what is gyro = _gyro_filtered;
// multiplied by time to integrate in DCM _have_gyro_sample = false;
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f; _have_accel_sample = false;
_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;
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s // Adjust for chip scaling to get m/s/s
_accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel, now);
// 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);
// Adjust for chip scaling to get radians/sec // Adjust for chip scaling to get radians/sec
_gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S; gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0]; _rotate_and_offset_gyro(_gyro_instance, gyro, now);
if (_last_filter_hz != _mpu6000_filter) { if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_mpu6000_filter); _set_filter_frequency(_imu.get_filter());
_last_filter_hz = _mpu6000_filter; _last_filter_hz = _imu.get_filter();
} }
return true; 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. // 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 // Its job is to accumulate samples as fast as is reasonable for the accel and gyro
// sensors. // sensors.
// Cant call this from within the system timers, since the long I2C reads (up to 1ms) // Note that this is called from gyro_sample_available() and
// with interrupts disabled breaks lots of things // accel_sample_available(), which is really not good enough for
// Therefore must call this as often as possible from // 800Hz, as it is common for the main loop to take more than 1.5ms
// within the mainline and thropttle the reads here to suit the sensors // 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) void AP_InertialSensor_Flymaple::_accumulate(void)
{ {
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
@ -285,7 +244,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
// Read accelerometer // Read accelerometer
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
uint8_t buffer[6]; uint8_t buffer[6];
uint64_t now = hal.scheduler->micros(); uint32_t now = hal.scheduler->micros();
// This takes about 250us at 400kHz I2C speed // This takes about 250us at 400kHz I2C speed
if ((now - _last_accel_timestamp) >= raw_sample_interval_us if ((now - _last_accel_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0) && 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_filtered = Vector3f(_accel_filter_x.apply(x),
_accel_filter_y.apply(y), _accel_filter_y.apply(y),
_accel_filter_z.apply(z)); _accel_filter_z.apply(z));
_accel_samples++; _have_accel_sample = true;
_last_accel_timestamp = now; _last_accel_timestamp = now;
} }
@ -317,7 +276,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
_gyro_filtered = Vector3f(_gyro_filter_x.apply(x), _gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
_gyro_filter_y.apply(y), _gyro_filter_y.apply(y),
_gyro_filter_z.apply(z)); _gyro_filter_z.apply(z));
_gyro_samples++; _have_gyro_sample = true;
_last_gyro_timestamp = now; _last_gyro_timestamp = now;
} }
@ -325,26 +284,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
i2c_sem->give(); 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 #endif // CONFIG_HAL_BOARD

57
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

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