Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: roll up to latest AP_InertialSensor from master

Browse Source
This commit is contained in:
Andrew Tridgell 2015-01-08 13:19:11 +11:00 committed by Randy Mackay
parent 0ad7a39db2
commit 827e60f948
19 changed files with 1381 additions and 1503 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

460
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -90,14 +90,126 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0), AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0),
#if INS_MAX_INSTANCES > 1 #if INS_MAX_INSTANCES > 1
// @Param: ACC2SCAL_X
// @DisplayName: Accelerometer2 scaling of X axis
// @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC2SCAL_Y
// @DisplayName: Accelerometer2 scaling of Y axis
// @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC2SCAL_Z
// @DisplayName: Accelerometer2 scaling of Z axis
// @Description: Accelerometer2 scaling of Z axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_GROUPINFO("ACC2SCAL", 5, AP_InertialSensor, _accel_scale[1], 0), AP_GROUPINFO("ACC2SCAL", 5, AP_InertialSensor, _accel_scale[1], 0),
// @Param: ACC2OFFS_X
// @DisplayName: Accelerometer2 offsets of X axis
// @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
// @Param: ACC2OFFS_Y
// @DisplayName: Accelerometer2 offsets of Y axis
// @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
// @Param: ACC2OFFS_Z
// @DisplayName: Accelerometer2 offsets of Z axis
// @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
AP_GROUPINFO("ACC2OFFS", 6, AP_InertialSensor, _accel_offset[1], 0), AP_GROUPINFO("ACC2OFFS", 6, AP_InertialSensor, _accel_offset[1], 0),
// @Param: GYR2OFFS_X
// @DisplayName: Gyro2 offsets of X axis
// @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
// @Param: GYR2OFFS_Y
// @DisplayName: Gyro2 offsets of Y axis
// @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
// @Param: GYR2OFFS_Z
// @DisplayName: Gyro2 offsets of Z axis
// @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0), AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0),
#endif #endif
#if INS_MAX_INSTANCES > 2 #if INS_MAX_INSTANCES > 2
// @Param: ACC3SCAL_X
// @DisplayName: Accelerometer3 scaling of X axis
// @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC3SCAL_Y
// @DisplayName: Accelerometer3 scaling of Y axis
// @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC3SCAL_Z
// @DisplayName: Accelerometer3 scaling of Z axis
// @Description: Accelerometer3 scaling of Z axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_GROUPINFO("ACC3SCAL", 8, AP_InertialSensor, _accel_scale[2], 0), AP_GROUPINFO("ACC3SCAL", 8, AP_InertialSensor, _accel_scale[2], 0),
// @Param: ACC3OFFS_X
// @DisplayName: Accelerometer3 offsets of X axis
// @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
// @Param: ACC3OFFS_Y
// @DisplayName: Accelerometer3 offsets of Y axis
// @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
// @Param: ACC3OFFS_Z
// @DisplayName: Accelerometer3 offsets of Z axis
// @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
// @Range: -300 300
// @User: Advanced
AP_GROUPINFO("ACC3OFFS", 9, AP_InertialSensor, _accel_offset[2], 0), AP_GROUPINFO("ACC3OFFS", 9, AP_InertialSensor, _accel_offset[2], 0),
// @Param: GYR3OFFS_X
// @DisplayName: Gyro3 offsets of X axis
// @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
// @Param: GYR3OFFS_Y
// @DisplayName: Gyro3 offsets of Y axis
// @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
// @Param: GYR3OFFS_Z
// @DisplayName: Gyro3 offsets of Z axis
// @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0), AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0),
#endif #endif
@ -105,30 +217,154 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
}; };
AP_InertialSensor::AP_InertialSensor() : AP_InertialSensor::AP_InertialSensor() :
_gyro_count(0),
_accel_count(0),
_backend_count(0),
_accel(), _accel(),
_gyro(), _gyro(),
_board_orientation(ROTATION_NONE) _board_orientation(ROTATION_NONE),
_hil_mode(false),
_have_3D_calibration(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_BACKENDS; i++) {
_backends[i] = NULL;
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_error_count[i] = 0;
_gyro_error_count[i] = 0;
}
}
/*
register a new gyro instance
*/
uint8_t AP_InertialSensor::register_gyro(void)
{
if (_gyro_count == INS_MAX_INSTANCES) {
hal.scheduler->panic(PSTR("Too many gyros"));
}
return _gyro_count++;
}
/*
register a new accel instance
*/
uint8_t AP_InertialSensor::register_accel(void)
{
if (_accel_count == INS_MAX_INSTANCES) {
hal.scheduler->panic(PSTR("Too many accels"));
}
return _accel_count++;
} }
void void
AP_InertialSensor::init( Start_style style, AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate) Sample_rate sample_rate)
{ {
_product_id = _init_sensor(sample_rate); // remember the sample rate
_sample_rate = sample_rate;
// check scaling if (_gyro_count == 0 && _accel_count == 0) {
// detect available backends. Only called once
_detect_backends();
}
_product_id = 0; // FIX
// initialise accel scale if need be. This is needed as we can't
// give non-zero default values for vectors in AP_Param
for (uint8_t i=0; i<get_accel_count(); i++) { for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) { if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1)); _accel_scale[i].set(Vector3f(1,1,1));
} }
} }
// remember whether we have 3D calibration so this can be used for
// AHRS health
check_3D_calibration();
if (WARM_START != style) { if (WARM_START != style) {
// do cold-start calibration for gyro only // do cold-start calibration for gyro only
_init_gyro(); _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;
}
// establish the baseline time between samples
_delta_time = 0;
_next_sample_usec = 0;
_last_sample_usec = 0;
_have_sample = false;
}
/*
try to load a backend
*/
void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &))
{
if (_backend_count == INS_MAX_BACKENDS) {
hal.scheduler->panic(PSTR("Too many INS backends"));
}
_backends[_backend_count] = detect(*this);
if (_backends[_backend_count] != NULL) {
_backend_count++;
}
}
/*
detect available backends for this board
*/
void
AP_InertialSensor::_detect_backends(void)
{
#if HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
_add_backend(AP_InertialSensor_MPU6000::detect);
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
_add_backend(AP_InertialSensor_PX4::detect);
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
_add_backend(AP_InertialSensor_Oilpan::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250
_add_backend(AP_InertialSensor_MPU9250::detect);
#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
_add_backend(AP_InertialSensor_Flymaple::detect);
#else
#error Unrecognised HAL_INS_TYPE setting
#endif
#if 0 // disabled due to broken hardware on some PXF capes
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
// the PXF also has a MPU6000
_add_backend(AP_InertialSensor_MPU6000::detect);
#endif
#endif
if (_backend_count == 0 ||
_gyro_count == 0 ||
_accel_count == 0) {
hal.scheduler->panic(PSTR("No INS backends available"));
}
// set the product ID to the ID of the first backend
_product_id.set(_backends[0]->product_id());
} }
void void
@ -138,6 +374,8 @@ AP_InertialSensor::init_accel()
// save calibration // save calibration
_save_parameters(); _save_parameters();
check_3D_calibration();
} }
#if !defined( __AVR_ATmega1280__ ) #if !defined( __AVR_ATmega1280__ )
@ -213,10 +451,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
} }
uint8_t num_samples = 0; uint8_t num_samples = 0;
while (num_samples < 32) { while (num_samples < 32) {
if (!wait_for_sample(1000)) { wait_for_sample();
interact->printf_P(PSTR("Failed to get INS sample\n"));
goto failed;
}
// read samples from ins // read samples from ins
update(); update();
// capture sample // capture sample
@ -254,6 +489,8 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
} }
_save_parameters(); _save_parameters();
check_3D_calibration();
// calculate the trims as well from primary accels and pass back to caller // calculate the trims as well from primary accels and pass back to caller
_calculate_trim(samples[0][0], trim_roll, trim_pitch); _calculate_trim(samples[0][0], trim_roll, trim_pitch);
@ -271,18 +508,37 @@ failed:
} }
#endif #endif
/// calibrated - returns true if the accelerometers have been calibrated /*
/// @note this should not be called while flying because it reads from the eeprom which can be slow check if the accelerometers are calibrated in 3D. Called on startup
bool AP_InertialSensor::calibrated() and any accel cal
*/
void AP_InertialSensor::check_3D_calibration()
{ {
_have_3D_calibration = false;
// check each accelerometer has offsets saved // check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) { for (uint8_t i=0; i<get_accel_count(); i++) {
if (!_accel_offset[i].load()) { // exactly 0.0 offset is extremely unlikely
return false; if (_accel_offset[i].get().is_zero()) {
return;
}
// exactly 1.0 scaling is extremely unlikely
const Vector3f &scaling = _accel_scale[i].get();
if (fabsf(scaling.x - 1.0f) < 0.00001f &&
fabsf(scaling.y - 1.0f) < 0.00001f &&
fabsf(scaling.z - 1.0f) < 0.00001f) {
return;
} }
} }
// if we got this far the accelerometers must have been calibrated // if we got this far the accelerometers must have been calibrated
return true; _have_3D_calibration = true;
}
/*
return true if we have 3D calibration values
*/
bool AP_InertialSensor::calibrated()
{
return _have_3D_calibration;
} }
void void
@ -463,9 +719,9 @@ AP_InertialSensor::_init_gyro()
uint8_t num_converged = 0; uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 10 seconds // we try to get a good calibration estimate for up to 30 seconds
// if the gyros are stable, we should get it in 1 second // if the gyros are stable, we should get it in 1 second
for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) { for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES]; Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
float diff_norm[INS_MAX_INSTANCES]; float diff_norm[INS_MAX_INSTANCES];
uint8_t i; uint8_t i;
@ -717,3 +973,177 @@ void AP_InertialSensor::_save_parameters()
_gyro_offset[i].save(); _gyro_offset[i].save();
} }
} }
/*
update gyro and accel values from backends
*/
void AP_InertialSensor::update(void)
{
// during initialisation update() may be called without
// wait_for_sample(), and a wait is implied
wait_for_sample();
if (!_hil_mode) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
// mark sensors unhealthy and let update() in each backend
// mark them healthy via _rotate_and_offset_gyro() and
// _rotate_and_offset_accel()
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
}
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
}
// adjust health status if a sensor has a non-zero error count
// but another sensor doesn't.
bool have_zero_accel_error_count = false;
bool have_zero_gyro_error_count = false;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _accel_error_count[i] == 0) {
have_zero_accel_error_count = true;
}
if (_gyro_healthy[i] && _gyro_error_count[i] == 0) {
have_zero_gyro_error_count = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) {
// we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false;
}
if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
}
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i]) {
_primary_gyro = i;
break;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i]) {
_primary_accel = i;
break;
}
}
}
_have_sample = false;
}
/*
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)
{
if (_have_sample) {
// the user has called wait_for_sample() again without
// consuming the sample with update()
return;
}
uint32_t now = hal.scheduler->micros();
if (_next_sample_usec == 0 && _delta_time <= 0) {
// this is the first call to wait_for_sample()
_last_sample_usec = now - _sample_period_usec;
_next_sample_usec = now + _sample_period_usec;
goto check_sample;
}
// see how long it is till the next sample is due
if (_next_sample_usec - now <=_sample_period_usec) {
// we're ahead on time, schedule next sample at expected period
uint32_t wait_usec = _next_sample_usec - now;
if (wait_usec > 200) {
hal.scheduler->delay_microseconds(wait_usec);
}
_next_sample_usec += _sample_period_usec;
} else if (now - _next_sample_usec < _sample_period_usec/8) {
// we've overshot, but only by a small amount, keep on
// schedule with no delay
_next_sample_usec += _sample_period_usec;
} else {
// we've overshot by a larger amount, re-zero scheduling with
// no delay
_next_sample_usec = now + _sample_period_usec;
}
check_sample:
if (!_hil_mode) {
// we 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<_backend_count; i++) {
gyro_available |= _backends[i]->gyro_sample_available();
accel_available |= _backends[i]->accel_sample_available();
}
if (!gyro_available || !accel_available) {
hal.scheduler->delay_microseconds(100);
}
}
}
now = hal.scheduler->micros();
_delta_time = (now - _last_sample_usec) * 1.0e-6f;
_last_sample_usec = now;
#if 0
{
static uint64_t delta_time_sum;
static uint16_t counter;
if (delta_time_sum == 0) {
delta_time_sum = _sample_period_usec;
}
delta_time_sum += _delta_time * 1.0e6f;
if (counter++ == 400) {
counter = 0;
hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n",
(unsigned long)now,
(unsigned long)delta_time_sum,
(long)(now - delta_time_sum));
}
}
#endif
_have_sample = true;
}
/*
support for setting accel and gyro vectors, for use by HIL
*/
void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
{
if (instance < INS_MAX_INSTANCES) {
_accel[instance] = accel;
_accel_healthy[instance] = true;
}
}
void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
{
if (instance < INS_MAX_INSTANCES) {
_gyro[instance] = gyro;
_gyro_healthy[instance] = true;
}
}

179
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -11,18 +11,22 @@
maximum number of INS instances available on this platform. If more maximum number of INS instances available on this platform. If more
than 1 then redundent sensors may be available than 1 then redundent sensors may be available
*/ */
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #if HAL_CPU_CLASS > HAL_CPU_CLASS_16
#define INS_MAX_INSTANCES 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#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>
#include "AP_InertialSensor_UserInteract.h" #include "AP_InertialSensor_UserInteract.h"
class AP_InertialSensor_Backend;
/* AP_InertialSensor is an abstraction for gyro and accel measurements /* AP_InertialSensor is an abstraction for gyro and accel measurements
* which are correctly aligned to the body axes and scaled to SI units. * which are correctly aligned to the body axes and scaled to SI units.
* *
@ -32,12 +36,11 @@
*/ */
class AP_InertialSensor class AP_InertialSensor
{ {
friend class AP_InertialSensor_Backend;
public: public:
AP_InertialSensor(); AP_InertialSensor();
// empty virtual destructor
virtual ~AP_InertialSensor() {}
enum Start_style { enum Start_style {
COLD_START = 0, COLD_START = 0,
WARM_START WARM_START
@ -64,7 +67,7 @@ public:
/// ///
/// @param style The initialisation startup style. /// @param style The initialisation startup style.
/// ///
virtual void init( Start_style style, void init( Start_style style,
Sample_rate sample_rate); Sample_rate sample_rate);
/// Perform cold startup initialisation for just the accelerometers. /// Perform cold startup initialisation for just the accelerometers.
@ -72,12 +75,18 @@ public:
/// @note This should not be called unless ::init has previously /// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work. /// been called, as ::init may perform other work.
/// ///
virtual void init_accel(); void init_accel();
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro(void);
uint8_t register_accel(void);
#if !defined( __AVR_ATmega1280__ ) #if !defined( __AVR_ATmega1280__ )
// perform accelerometer calibration including providing user instructions // perform accelerometer calibration including providing user instructions
// and feedback // and feedback
virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact, bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
float& trim_roll, float& trim_roll,
float& trim_pitch); float& trim_pitch);
#endif #endif
@ -93,65 +102,66 @@ public:
/// @note This should not be called unless ::init has previously /// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work /// been called, as ::init may perform other work
/// ///
virtual void init_gyro(void); void init_gyro(void);
/// Fetch the current gyro values /// Fetch the current gyro values
/// ///
/// @returns vector of rotational rates in radians/sec /// @returns vector of rotational rates in radians/sec
/// ///
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; } const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); } const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
virtual void set_gyro(uint8_t instance, const Vector3f &gyro) {} void set_gyro(uint8_t instance, const Vector3f &gyro);
// set gyro offsets in radians/sec // set gyro offsets in radians/sec
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; } const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); } const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
/// Fetch the current accelerometer values /// Fetch the current accelerometer values
/// ///
/// @returns vector of current accelerations in m/s/s /// @returns vector of current accelerations in m/s/s
/// ///
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; } const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
const Vector3f &get_accel(void) const { return get_accel(get_primary_accel()); } const Vector3f &get_accel(void) const { return get_accel(_primary_accel); }
virtual void set_accel(uint8_t instance, const Vector3f &accel) {} void set_accel(uint8_t instance, const Vector3f &accel);
uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
// multi-device interface // multi-device interface
virtual bool get_gyro_health(uint8_t instance) const { return true; } bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
bool get_gyro_health_all(void) const; bool get_gyro_health_all(void) const;
virtual uint8_t get_gyro_count(void) const { return 1; }; uint8_t get_gyro_count(void) const { return _gyro_count; }
bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; } bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
bool gyro_calibrated_ok_all() const; bool gyro_calibrated_ok_all() const;
virtual bool get_accel_health(uint8_t instance) const { return true; } bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; }
bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
bool get_accel_health_all(void) const; bool get_accel_health_all(void) const;
virtual uint8_t get_accel_count(void) const { return 1; }; uint8_t get_accel_count(void) const { return _accel_count; };
// get accel offsets in m/s/s // get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; } const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(get_primary_accel()); } const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
// get accel scale // get accel scale
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; } const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
const Vector3f &get_accel_scale(void) const { return get_accel_scale(get_primary_accel()); } const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not.
*/
virtual bool update() = 0;
/* get_delta_time returns the time period in seconds /* get_delta_time returns the time period in seconds
* overwhich the sensor data was collected * overwhich the sensor data was collected
*/ */
virtual float get_delta_time() const = 0; float get_delta_time() const { return _delta_time; }
// return the maximum gyro drift rate in radians/s/s. This // return the maximum gyro drift rate in radians/s/s. This
// depends on what gyro chips are being used // depends on what gyro chips are being used
virtual float get_gyro_drift_rate(void) = 0; float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); }
// wait for a sample to be available, with timeout in milliseconds // update gyro and accel values from accumulated samples
virtual bool wait_for_sample(uint16_t timeout_ms) = 0; void update(void);
// wait for a sample to be available
void wait_for_sample(void);
// class level parameters // class level parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -169,24 +179,28 @@ public:
} }
// get_filter - return filter in hz // get_filter - return filter in hz
virtual uint8_t get_filter() const { return _mpu6000_filter.get(); } uint8_t get_filter() const { return _mpu6000_filter.get(); }
virtual uint16_t error_count(void) const { return 0; } // return the selected sample rate
virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); } Sample_rate get_sample_rate(void) const { return _sample_rate; }
virtual uint8_t get_primary_accel(void) const { return 0; } uint16_t error_count(void) const { return 0; }
bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
protected: uint8_t get_primary_accel(void) const { return 0; }
virtual uint8_t _get_primary_gyro(void) const { return 0; } // enable HIL mode
void set_hil_mode(void) { _hil_mode = true; }
// sensor specific init to be overwritten by descendant classes private:
virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0;
// no-save implementations of accel and gyro initialisation routines // load backend drivers
virtual void _init_accel(); void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
void _detect_backends(void);
virtual void _init_gyro(); // accel and gyro initialisation
void _init_accel();
void _init_gyro();
#if !defined( __AVR_ATmega1280__ ) #if !defined( __AVR_ATmega1280__ )
// Calibration routines borrowed from Rolfe Schmidt // Calibration routines borrowed from Rolfe Schmidt
@ -194,23 +208,36 @@ protected:
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
// _calibrate_accel - perform low level accel calibration // _calibrate_accel - perform low level accel calibration
virtual bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
virtual void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
virtual void _calibrate_reset_matrices(float dS[6], float JS[6][6]); void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
virtual void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
virtual void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
#endif #endif
// check if we have 3D accel calibration
void check_3D_calibration(void);
// save parameters to eeprom // save parameters to eeprom
void _save_parameters(); void _save_parameters();
// Most recent accelerometer reading obtained by ::update // backend objects
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
// counters on initialisation
uint8_t _gyro_count;
uint8_t _accel_count;
uint8_t _backend_count;
// the selected sample rate
Sample_rate _sample_rate;
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES]; Vector3f _accel[INS_MAX_INSTANCES];
// previous accelerometer reading obtained by ::update // Most recent gyro reading
Vector3f _previous_accel[INS_MAX_INSTANCES];
// Most recent gyro reading obtained by ::update
Vector3f _gyro[INS_MAX_INSTANCES]; Vector3f _gyro[INS_MAX_INSTANCES];
// product id // product id
@ -229,18 +256,50 @@ protected:
// calibrated_ok flags // calibrated_ok flags
bool _gyro_cal_ok[INS_MAX_INSTANCES]; bool _gyro_cal_ok[INS_MAX_INSTANCES];
// primary accel and gyro
uint8_t _primary_gyro;
uint8_t _primary_accel;
// has wait_for_sample() found a sample?
bool _have_sample:1;
// are we in HIL mode?
bool _hil_mode:1;
// do we have offsets/scaling from a 3D calibration?
bool _have_3D_calibration:1;
// the delta time in seconds for the last sample
float _delta_time;
// last time a wait_for_sample() returned a sample
uint32_t _last_sample_usec;
// target time for next wait_for_sample() return
uint32_t _next_sample_usec;
// time between samples in microseconds
uint32_t _sample_period_usec;
// health of gyros and accels
bool _gyro_healthy[INS_MAX_INSTANCES];
bool _accel_healthy[INS_MAX_INSTANCES];
uint32_t _accel_error_count[INS_MAX_INSTANCES];
uint32_t _gyro_error_count[INS_MAX_INSTANCES];
}; };
#include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_Backend.h"
#include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_MPU6000.h"
#include "AP_InertialSensor_HIL.h"
#include "AP_InertialSensor_PX4.h" #include "AP_InertialSensor_PX4.h"
#include "AP_InertialSensor_VRBRAIN.h" #include "AP_InertialSensor_Oilpan.h"
#include "AP_InertialSensor_MPU9250.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_Flymaple.h"
#include "AP_InertialSensor_MPU9150.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"
#include "AP_InertialSensor_Flymaple.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_MPU9150.h"
#include "AP_InertialSensor_MPU9250.h"
#endif // __AP_INERTIAL_SENSOR_H__ #endif // __AP_INERTIAL_SENSOR_H__

204
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,25 +63,39 @@ 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)
{ {
// Sensors are raw sampled at 800Hz. AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu);
// Here we figure the divider to get the rate that update should be called if (sensor == NULL) {
switch (sample_rate) { return NULL;
case RATE_50HZ:
_sample_divider = raw_sample_rate_hz / 50;
_default_filter_hz = 10;
break;
case RATE_100HZ:
_sample_divider = raw_sample_rate_hz / 100;
_default_filter_hz = 20;
break;
case RATE_200HZ:
default:
_sample_divider = raw_sample_rate_hz / 200;
_default_filter_hz = 20;
break;
} }
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_Flymaple::_init_sensor(void)
{
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -146,12 +146,17 @@ 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();
_product_id = AP_PRODUCT_ID_FLYMAPLE;
return true;
} }
/* /*
@ -170,109 +175,46 @@ 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;
}
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 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);
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 +227,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 +242,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 +259,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 +267,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

56
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -6,39 +6,31 @@
#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 */
/* Concrete implementation of AP_InertialSensor functions: */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate(); bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
bool get_gyro_health(void) const;
bool get_accel_health(void) const; // detect the sensor
bool healthy(void) const { return get_gyro_health() && get_accel_health(); } static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private: private:
uint16_t _init_sensor( Sample_rate sample_rate ); bool _init_sensor(void);
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 +38,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__

140
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -1,130 +1,46 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_InertialSensor_HIL.h"
#include <AP_HAL.h> #include <AP_HAL.h>
#include "AP_InertialSensor_HIL.h"
const extern AP_HAL::HAL& hal; const extern AP_HAL::HAL& hal;
AP_InertialSensor_HIL::AP_InertialSensor_HIL() : AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu)
_sample_period_usec(0),
_last_sample_usec(0)
{ {
_accel[0] = Vector3f(0, 0, -GRAVITY_MSS);
} }
uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) { /*
switch (sample_rate) { detect the sensor
case RATE_50HZ: */
_sample_period_usec = 20000; AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu)
break; {
case RATE_100HZ: AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
_sample_period_usec = 10000; if (sensor == NULL) {
break; return NULL;
case RATE_200HZ:
_sample_period_usec = 5000;
break;
case RATE_400HZ:
_sample_period_usec = 2500;
break;
} }
return AP_PRODUCT_ID_NONE; if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ bool AP_InertialSensor_HIL::_init_sensor(void)
{
// grab the used instances
_imu.register_gyro();
_imu.register_accel();
_product_id = AP_PRODUCT_ID_NONE;
_imu.set_hil_mode();
bool AP_InertialSensor_HIL::update( void ) {
uint32_t now = hal.scheduler->micros();
_last_sample_usec = now;
return true; return true;
} }
float AP_InertialSensor_HIL::get_delta_time() const { bool AP_InertialSensor_HIL::update(void)
return _sample_period_usec * 1.0e-6f;
}
float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
bool AP_InertialSensor_HIL::_sample_available()
{ {
uint32_t tnow = hal.scheduler->micros(); // the data is stored directly in the frontend, so update()
bool have_sample = false; // doesn't need to do anything
while (tnow - _last_sample_usec > _sample_period_usec) {
have_sample = true;
_last_sample_usec += _sample_period_usec;
}
return have_sample;
}
bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
{
if (_sample_available()) {
return true; return true;
}
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) {
uint32_t tnow = hal.scheduler->micros();
uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
if (tdelay < 100000) {
hal.scheduler->delay_microseconds(tdelay);
}
if (_sample_available()) {
return true;
}
}
return false;
} }
void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
{
if (instance >= INS_MAX_INSTANCES) {
return;
}
_previous_accel[instance] = _accel[instance];
_accel[instance] = accel;
_last_accel_usec[instance] = hal.scheduler->micros();
}
void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
{
if (instance >= INS_MAX_INSTANCES) {
return;
}
_gyro[instance] = gyro;
_last_gyro_usec[instance] = hal.scheduler->micros();
}
bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
}
bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
}
uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
{
if (get_gyro_health(1)) {
return 2;
}
return 1;
}
uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
{
if (get_accel_health(1)) {
return 2;
}
return 1;
}

36
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -1,36 +1,26 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_INERTIAL_SENSOR_STUB_H__ #ifndef __AP_INERTIALSENSOR_HIL_H__
#define __AP_INERTIAL_SENSOR_STUB_H__ #define __AP_INERTIALSENSOR_HIL_H__
#include <AP_Progmem.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
class AP_InertialSensor_HIL : public AP_InertialSensor class AP_InertialSensor_HIL : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_HIL(AP_InertialSensor &imu);
AP_InertialSensor_HIL(); /* update accel and gyro state */
/* Concrete implementation of AP_InertialSensor functions: */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate(); bool gyro_sample_available(void) { return true; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return true; }
void set_accel(uint8_t instance, const Vector3f &accel);
void set_gyro(uint8_t instance, const Vector3f &gyro); // detect the sensor
bool get_gyro_health(uint8_t instance) const; static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
bool get_accel_health(uint8_t instance) const;
uint8_t get_gyro_count(void) const;
uint8_t get_accel_count(void) const;
private: private:
bool _sample_available(); bool _init_sensor(void);
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_usec;
uint32_t _last_sample_usec;
uint32_t _last_accel_usec[INS_MAX_INSTANCES];
uint32_t _last_gyro_usec[INS_MAX_INSTANCES];
}; };
#endif // __AP_INERTIAL_SENSOR_STUB_H__ #endif // __AP_INERTIALSENSOR_HIL_H__

134
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -16,9 +16,17 @@
/* /*
This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer. This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
This combination is available as a cheap 10DOF sensor on ebay This combination is available as a cheap 10DOF sensor on ebay
*/
// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf This sensor driver is an example only - it should not be used in any
// L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf serious autopilot as the latencies on I2C prevent good timing at
high sample rates. It is useful when doing an initial port of
ardupilot to a board where only I2C is available, and a cheap sensor
can be used.
Datasheets:
ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
*/
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
@ -103,8 +111,10 @@ const extern AP_HAL::HAL& hal;
#define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f) #define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f)
// constructor // constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() : AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu),
_have_gyro_sample(false),
_have_accel_sample(false),
_accel_filter_x(800, 10), _accel_filter_x(800, 10),
_accel_filter_y(800, 10), _accel_filter_y(800, 10),
_accel_filter_z(800, 10), _accel_filter_z(800, 10),
@ -113,27 +123,9 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
_gyro_filter_z(800, 10) _gyro_filter_z(800, 10)
{} {}
uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) bool AP_InertialSensor_L3G4200D::_init_sensor(void)
{ {
_default_filter_hz = _default_filter();
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 10;
_sample_period_usec = (1000*1000) / 50;
_gyro_samples_needed = 16;
break;
case RATE_100HZ:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 100;
_gyro_samples_needed = 8;
break;
case RATE_200HZ:
default:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 200;
_gyro_samples_needed = 4;
break;
}
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -219,7 +211,7 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
// 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();
@ -227,15 +219,12 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
// start the timer process to read samples // start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate)); hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate));
clock_gettime(CLOCK_MONOTONIC, &_next_sample_ts); _gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_next_sample_ts.tv_nsec += _sample_period_usec * 1000UL; _product_id = AP_PRODUCT_ID_L3G4200D;
if (_next_sample_ts.tv_nsec >= 1.0e9) {
_next_sample_ts.tv_nsec -= 1.0e9;
_next_sample_ts.tv_sec++;
}
return AP_PRODUCT_ID_L3G4200D; return true;
} }
/* /*
@ -254,58 +243,36 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
_gyro_filter_z.set_cutoff_frequency(800, filter_hz); _gyro_filter_z.set_cutoff_frequency(800, filter_hz);
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ /*
copy filtered data to the frontend
*/
bool AP_InertialSensor_L3G4200D::update(void) bool AP_InertialSensor_L3G4200D::update(void)
{ {
Vector3f accel_scale = _accel_scale[0].get(); Vector3f accel, gyro;
_previous_accel[0] = _accel[0];
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
_accel[0] = _accel_filtered; accel = _accel_filtered;
_gyro[0] = _gyro_filtered; gyro = _gyro_filtered;
_delta_time = _gyro_samples_available * (1.0f/800); _have_gyro_sample = false;
_gyro_samples_available = 0; _have_accel_sample = false;
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] *= ADXL345_ACCELEROMETER_SCALE_M_S; accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel);
// 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] *= L3G4200D_GYRO_SCALE_R_S; gyro *= L3G4200D_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0]; _rotate_and_offset_gyro(_gyro_instance, gyro);
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;
} }
float AP_InertialSensor_L3G4200D::get_delta_time(void) const
{
return _delta_time;
}
float AP_InertialSensor_L3G4200D::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute (a guess)
return ToRad(0.5/60);
}
// Accumulate values from accels and gyros // Accumulate values from accels and gyros
void AP_InertialSensor_L3G4200D::_accumulate(void) void AP_InertialSensor_L3G4200D::_accumulate(void)
{ {
@ -344,19 +311,17 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]), _gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]),
_gyro_filter_y.apply(-buffer[i][1]), _gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2])); _gyro_filter_z.apply(-buffer[i][2]));
_gyro_samples_available++; _have_gyro_sample = true;
} }
} }
} }
// Read accelerometer FIFO to find out how many samples are available // Read accelerometer FIFO to find out how many samples are available
hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS, hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS,
ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS, ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS,
&fifo_status); &fifo_status);
num_samples_available = fifo_status & 0x3F; num_samples_available = fifo_status & 0x3F;
#if 1
// read the samples and apply the filter // read the samples and apply the filter
if (num_samples_available > 0) { if (num_samples_available > 0) {
int16_t buffer[num_samples_available][3]; int16_t buffer[num_samples_available][3];
@ -368,35 +333,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
_accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]), _accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]),
_accel_filter_y.apply(-buffer[i][1]), _accel_filter_y.apply(-buffer[i][1]),
_accel_filter_z.apply(-buffer[i][2])); _accel_filter_z.apply(-buffer[i][2]));
_have_accel_sample = true;
} }
} }
} }
#endif
// give back i2c semaphore // give back i2c semaphore
i2c_sem->give(); i2c_sem->give();
} }
bool AP_InertialSensor_L3G4200D::_sample_available(void)
{
return (_gyro_samples_available >= _gyro_samples_needed);
}
bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
{
uint32_t start_us = hal.scheduler->micros();
while (clock_nanosleep(CLOCK_MONOTONIC,
TIMER_ABSTIME, &_next_sample_ts, NULL) == -1 && errno == EINTR) ;
_next_sample_ts.tv_nsec += _sample_period_usec * 1000UL;
if (_next_sample_ts.tv_nsec >= 1.0e9) {
_next_sample_ts.tv_nsec -= 1.0e9;
_next_sample_ts.tv_sec++;
}
//_accumulate();
return true;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

35
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -6,36 +6,35 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#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_L3G4200D : public AP_InertialSensor class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_L3G4200D(); AP_InertialSensor_L3G4200D(AP_InertialSensor &imu);
/* Concrete implementation of AP_InertialSensor functions: */ /* update accel and gyro state */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate(); bool gyro_sample_available(void) { return _have_gyro_sample; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return _have_accel_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
// return product ID
int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; }
private: private:
uint16_t _init_sensor( Sample_rate sample_rate ); bool _init_sensor(void);
void _accumulate(void); void _accumulate(void);
bool _sample_available();
uint64_t _last_update_usec;
Vector3f _accel_filtered; Vector3f _accel_filtered;
Vector3f _gyro_filtered; Vector3f _gyro_filtered;
uint32_t _sample_period_usec; volatile bool _have_gyro_sample;
uint32_t _last_sample_time; volatile bool _have_accel_sample;
volatile uint32_t _gyro_samples_available;
volatile uint8_t _gyro_samples_needed;
float _delta_time;
struct timespec _next_sample_ts;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; uint8_t _last_filter_hz;
@ -50,6 +49,10 @@ private:
LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z; LowPassFilter2p _gyro_filter_z;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;
}; };
#endif #endif
#endif // __AP_INERTIAL_SENSOR_L3G4200D_H__ #endif // __AP_INERTIAL_SENSOR_L3G4200D_H__

276
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -173,26 +173,51 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
* variants however * variants however
*/ */
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu),
_drdy_pin(NULL), _drdy_pin(NULL),
_spi(NULL), _spi(NULL),
_spi_sem(NULL), _spi_sem(NULL),
_num_samples(0),
_last_sample_time_micros(0),
_initialised(false),
_mpu6000_product_id(AP_PRODUCT_ID_NONE),
_sample_shift(0),
_last_filter_hz(0), _last_filter_hz(0),
_error_count(0) _error_count(0),
#if MPU6000_FAST_SAMPLING
_accel_filter_x(1000, 15),
_accel_filter_y(1000, 15),
_accel_filter_z(1000, 15),
_gyro_filter_x(1000, 15),
_gyro_filter_y(1000, 15),
_gyro_filter_z(1000, 15),
#else
_sample_count(0),
_accel_sum(),
_gyro_sum(),
#endif
_sum_count(0)
{ {
} }
uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) /*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
{ {
if (_initialised) return _mpu6000_product_id; AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
_initialised = true; if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU6000::_init_sensor(void)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000); _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
@ -205,103 +230,85 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
uint8_t tries = 0; uint8_t tries = 0;
do { do {
bool success = _hardware_init(sample_rate); bool success = _hardware_init();
if (success) { if (success) {
hal.scheduler->delay(5+2); hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); return false;
} }
if (_data_ready()) { if (_data_ready()) {
_spi_sem->give(); _spi_sem->give();
break; break;
} else { } else {
hal.console->println_P( return false;
PSTR("MPU6000 startup failed: no data ready"));
} }
_spi_sem->give(); _spi_sem->give();
} }
if (tries++ > 5) { if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times")); hal.console->print_P(PSTR("failed to boot MPU6000 5 times"));
return false;
} }
} while (1); } while (1);
// grab the used instances
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples // start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data)); hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data));
#if MPU6000_DEBUG #if MPU6000_DEBUG
_dump_registers(); _dump_registers();
#endif #endif
return _mpu6000_product_id;
return true;
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_MPU6000::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;
}
/*
process any
*/
bool AP_InertialSensor_MPU6000::update( void ) bool AP_InertialSensor_MPU6000::update( void )
{ {
// wait for at least 1 sample #if !MPU6000_FAST_SAMPLING
if (!wait_for_sample(1000)) { if (_sum_count < _sample_count) {
// we don't have enough samples yet
return false; return false;
} }
#endif
_previous_accel[0] = _accel[0]; // we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); #if MPU6000_FAST_SAMPLING
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); gyro = _gyro_filtered;
_num_samples = _sum_count; accel = _accel_filtered;
num_samples = 1;
#else
gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
accel(_accel_sum.x, _accel_sum.y, _accel_sum.z);
num_samples = _sum_count;
_accel_sum.zero(); _accel_sum.zero();
_gyro_sum.zero(); _gyro_sum.zero();
#endif
_sum_count = 0; _sum_count = 0;
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation); gyro *= _gyro_scale / num_samples;
_gyro[0] *= _gyro_scale / _num_samples; _rotate_and_offset_gyro(_gyro_instance, gyro);
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation); accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
_accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; _rotate_and_offset_accel(_accel_instance, accel);
Vector3f accel_scale = _accel_scale[0].get(); if (_last_filter_hz != _imu.get_filter()) {
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) { if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0); _set_filter_register(_imu.get_filter());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give(); _spi_sem->give();
} }
} }
@ -331,35 +338,13 @@ bool AP_InertialSensor_MPU6000::_data_ready()
*/ */
void AP_InertialSensor_MPU6000::_poll_data(void) void AP_InertialSensor_MPU6000::_poll_data(void)
{ {
if (hal.scheduler->in_timerprocess()) {
if (!_spi_sem->take_nonblocking()) { if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
return; return;
} }
if (_data_ready()) { if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction(); _read_data_transaction();
} }
_spi_sem->give(); _spi_sem->give();
} else {
/* Synchronous read - take semaphore */
if (_spi_sem->take(10)) {
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
} }
@ -390,19 +375,31 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() {
} }
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
#if MPU6000_FAST_SAMPLING
_accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
_accel_filter_y.apply(int16_val(rx.v, 0)),
_accel_filter_z.apply(-int16_val(rx.v, 2)));
_gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)),
_gyro_filter_y.apply(int16_val(rx.v, 4)),
_gyro_filter_z.apply(-int16_val(rx.v, 6)));
#else
_accel_sum.x += int16_val(rx.v, 1); _accel_sum.x += int16_val(rx.v, 1);
_accel_sum.y += int16_val(rx.v, 0); _accel_sum.y += int16_val(rx.v, 0);
_accel_sum.z -= int16_val(rx.v, 2); _accel_sum.z -= int16_val(rx.v, 2);
_gyro_sum.x += int16_val(rx.v, 5); _gyro_sum.x += int16_val(rx.v, 5);
_gyro_sum.y += int16_val(rx.v, 4); _gyro_sum.y += int16_val(rx.v, 4);
_gyro_sum.z -= int16_val(rx.v, 6); _gyro_sum.z -= int16_val(rx.v, 6);
#endif
_sum_count++; _sum_count++;
#if !MPU6000_FAST_SAMPLING
if (_sum_count == 0) { if (_sum_count == 0) {
// rollover - v unlikely // rollover - v unlikely
_accel_sum.zero(); _accel_sum.zero();
_gyro_sum.zero(); _gyro_sum.zero();
} }
#endif
} }
uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg ) uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg )
@ -448,36 +445,30 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val)
/* /*
set the DLPF filter frequency. Assumes caller has taken semaphore set the DLPF filter frequency. Assumes caller has taken semaphore
*/ */
void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz)
{ {
uint8_t filter = default_filter; if (filter_hz == 0) {
// choose filtering frequency filter_hz = _default_filter();
switch (filter_hz) { }
case 5: uint8_t filter;
filter = BITS_DLPF_CFG_5HZ; // choose filtering frequency
break; if (filter_hz <= 5) {
case 10: filter = BITS_DLPF_CFG_5HZ;
filter = BITS_DLPF_CFG_10HZ; } else if (filter_hz <= 10) {
break; filter = BITS_DLPF_CFG_10HZ;
case 20: } else if (filter_hz <= 20) {
filter = BITS_DLPF_CFG_20HZ; filter = BITS_DLPF_CFG_20HZ;
break; } else if (filter_hz <= 42) {
case 42: filter = BITS_DLPF_CFG_42HZ;
filter = BITS_DLPF_CFG_42HZ; } else {
break; filter = BITS_DLPF_CFG_98HZ;
case 98:
filter = BITS_DLPF_CFG_98HZ;
break;
} }
if (filter != 0) {
_last_filter_hz = filter_hz; _last_filter_hz = filter_hz;
_register_write(MPUREG_CONFIG, filter); _register_write(MPUREG_CONFIG, filter);
}
} }
bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) bool AP_InertialSensor_MPU6000::_hardware_init(void)
{ {
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
@ -519,46 +510,53 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1); hal.scheduler->delay(1);
uint8_t default_filter; #if MPU6000_FAST_SAMPLING
_sample_count = 1;
#else
// sample rate and filtering // sample rate and filtering
// to minimise the effects of aliasing we choose a filter // to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate // that is less than half of the sample rate
switch (sample_rate) { switch (_imu.get_sample_rate()) {
case RATE_50HZ: case AP_InertialSensor::RATE_50HZ:
// this is used for plane and rover, where noise resistance is // this is used for plane and rover, where noise resistance is
// more important than update rate. Tests on an aerobatic plane // more important than update rate. Tests on an aerobatic plane
// show that 10Hz is fine, and makes it very noise resistant // show that 10Hz is fine, and makes it very noise resistant
default_filter = BITS_DLPF_CFG_10HZ; _sample_count = 4;
_sample_shift = 2;
break; break;
case RATE_100HZ: case AP_InertialSensor::RATE_100HZ:
default_filter = BITS_DLPF_CFG_20HZ; _sample_count = 2;
_sample_shift = 1; break;
case AP_InertialSensor::RATE_200HZ:
_sample_count = 1;
break; break;
case RATE_200HZ:
default: default:
default_filter = BITS_DLPF_CFG_20HZ; return false;
_sample_shift = 0;
break;
} }
#endif
_set_filter_register(_mpu6000_filter, default_filter); _set_filter_register(_imu.get_filter());
#if MPU6000_FAST_SAMPLING
// set sample rate to 1000Hz and apply a software filter
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
#else
// set sample rate to 200Hz, and use _sample_divider to give // set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application // the requested rate to the application
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
#endif
hal.scheduler->delay(1); hal.scheduler->delay(1);
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
hal.scheduler->delay(1); hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d // read the product ID rev c has 1/2 the sensitivity of rev d
_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); _product_id = _register_read(MPUREG_PRODUCT_ID);
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || if ((_product_id == MPU6000ES_REV_C4) ||
(_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { (_product_id == MPU6000ES_REV_C5) ||
(_product_id == MPU6000_REV_C4) ||
(_product_id == MPU6000_REV_C5)) {
// Accel scale 8g (4096 LSB/g) // Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D // Rev C has different scaling than rev D
_register_write(MPUREG_ACCEL_CONFIG,1<<3); _register_write(MPUREG_ACCEL_CONFIG,1<<3);
@ -585,22 +583,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
return true; return true;
} }
// return the MPU6k gyro drift rate in radian/s/s
// note that this is much better than the oilpan gyros
float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
// return true if a sample is available
bool AP_InertialSensor_MPU6000::_sample_available()
{
_poll_data();
return (_sum_count >> _sample_shift) > 0;
}
#if MPU6000_DEBUG #if MPU6000_DEBUG
// dump all config registers - used for debug // dump all config registers - used for debug
void AP_InertialSensor_MPU6000::_dump_registers(void) void AP_InertialSensor_MPU6000::_dump_registers(void)
@ -619,11 +601,3 @@ void AP_InertialSensor_MPU6000::_dump_registers(void)
} }
} }
#endif #endif
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
float AP_InertialSensor_MPU6000::get_delta_time() const
{
// the sensor runs at 200Hz
return 0.005 * _num_samples;
}

82
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -12,33 +12,44 @@
// enable debug to see a register dump on startup // enable debug to see a register dump on startup
#define MPU6000_DEBUG 0 #define MPU6000_DEBUG 0
class AP_InertialSensor_MPU6000 : public AP_InertialSensor // on fast CPUs we sample at 1kHz and use a software filter
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define MPU6000_FAST_SAMPLING 1
#else
#define MPU6000_FAST_SAMPLING 0
#endif
#if MPU6000_FAST_SAMPLING
#include <Filter.h>
#include <LowPassFilter2p.h>
#endif
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_MPU6000(AP_InertialSensor &imu);
AP_InertialSensor_MPU6000(); /* update accel and gyro state */
/* Concrete implementation of AP_InertialSensor functions: */
bool update(); bool update();
float get_gyro_drift_rate();
// wait for a sample to be available, with timeout in milliseconds bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return _sum_count >= _sample_count; }
// get_delta_time returns the time period in seconds overwhich the sensor data was collected // detect the sensor
float get_delta_time() const; static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) const { return _error_count <= 4; }
bool get_gyro_health(uint8_t instance) const { return healthy(); }
bool get_accel_health(uint8_t instance) const { return healthy(); }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );
private: private:
#if MPU6000_DEBUG
void _dump_registers(void);
#endif
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
uint8_t _accel_instance;
AP_HAL::DigitalSource *_drdy_pin; AP_HAL::DigitalSource *_drdy_pin;
bool _init_sensor(void);
bool _sample_available(); bool _sample_available();
void _read_data_transaction(); void _read_data_transaction();
bool _data_ready(); bool _data_ready();
@ -46,41 +57,42 @@ private:
uint8_t _register_read( uint8_t reg ); uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val ); void _register_write( uint8_t reg, uint8_t val );
void _register_write_check(uint8_t reg, uint8_t val); void _register_write_check(uint8_t reg, uint8_t val);
bool _hardware_init(Sample_rate sample_rate); bool _hardware_init(void);
AP_HAL::SPIDeviceDriver *_spi; AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem; AP_HAL::Semaphore *_spi_sem;
uint16_t _num_samples;
static const float _gyro_scale; static const float _gyro_scale;
uint32_t _last_sample_time_micros;
// ensure we can't initialise twice
bool _initialised;
int16_t _mpu6000_product_id;
// how many hardware samples before we report a sample to the caller
uint8_t _sample_shift;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; uint8_t _last_filter_hz;
void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); void _set_filter_register(uint8_t filter_hz);
// count of bus errors
uint16_t _error_count; uint16_t _error_count;
// how many hardware samples before we report a sample to the caller
uint8_t _sample_count;
#if MPU6000_FAST_SAMPLING
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
// Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x;
LowPassFilter2p _accel_filter_y;
LowPassFilter2p _accel_filter_z;
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
#else
// accumulation in timer - must be read with timer disabled // accumulation in timer - must be read with timer disabled
// the sum of the values since last read // the sum of the values since last read
Vector3l _accel_sum; Vector3l _accel_sum;
Vector3l _gyro_sum; Vector3l _gyro_sum;
volatile int16_t _sum_count;
public:
#if MPU6000_DEBUG
void _dump_registers(void);
#endif #endif
volatile uint16_t _sum_count;
}; };
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__ #endif // __AP_INERTIAL_SENSOR_MPU6000_H__

185
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -19,6 +19,10 @@
Please check the following links for datasheets and documentation: Please check the following links for datasheets and documentation:
- http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf
- http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf
Note that this is an experimental driver. It is not used by any
actively maintained board and should be considered untested and
unmaintained
*/ */
#include <AP_HAL.h> #include <AP_HAL.h>
@ -320,19 +324,33 @@ static struct gyro_state_s st = {
/** /**
* @brief Constructor * @brief Constructor
*/ */
AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() : AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu),
_have_sample_available(false),
_accel_filter_x(800, 10), _accel_filter_x(800, 10),
_accel_filter_y(800, 10), _accel_filter_y(800, 10),
_accel_filter_z(800, 10), _accel_filter_z(800, 10),
_gyro_filter_x(800, 10), _gyro_filter_x(800, 10),
_gyro_filter_y(800, 10), _gyro_filter_y(800, 10),
_gyro_filter_z(800, 10) _gyro_filter_z(800, 10)
// _mag_filter_x(800, 10),
// _mag_filter_y(800, 10),
// _mag_filter_z(800, 10)
{ {
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
} }
/* /*
@ -352,41 +370,21 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
} }
/** /**
* @brief Init method * Init method
* @param[in] Sample_rate The sample rate, check the struct def.
* @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
*/ */
uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) bool AP_InertialSensor_MPU9150::_init_sensor(void)
{ {
// Sensors pushed to the FIFO. // Sensors pushed to the FIFO.
uint8_t sensors; uint8_t sensors;
switch (sample_rate) { _default_filter_hz = _default_filter();
case RATE_50HZ:
_default_filter_hz = 10;
_sample_period_usec = (1000*1000) / 50;
break;
case RATE_100HZ:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 100;
break;
case RATE_200HZ:
_default_filter_hz = 20;
_sample_period_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 20;
_sample_period_usec = 2500;
break;
}
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore // take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){ if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){
return -1; return false;
} }
// Init the sensor // Init the sensor
@ -405,7 +403,7 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
// This registers are not documented in the register map. // This registers are not documented in the register map.
uint8_t buff[6]; uint8_t buff[6];
if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) { if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) {
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision")); hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision");
goto failed; goto failed;
} }
uint8_t rev; uint8_t rev;
@ -432,28 +430,28 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
// Set gyro full-scale range [250, 500, 1000, 2000] // Set gyro full-scale range [250, 500, 1000, 2000]
if (mpu_set_gyro_fsr(2000)){ if (mpu_set_gyro_fsr(2000)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n")); hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n");
goto failed; goto failed;
} }
// Set the accel full-scale range // Set the accel full-scale range
if (mpu_set_accel_fsr(2)){ if (mpu_set_accel_fsr(2)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n")); hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n");
goto failed; goto failed;
} }
// Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate) // Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate)
if (mpu_set_lpf(_default_filter_hz)){ if (mpu_set_lpf(_default_filter_hz)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n")); hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n");
goto failed; goto failed;
} }
// Set sampling rate (value must be between 4Hz and 1KHz) // Set sampling rate (value must be between 4Hz and 1KHz)
if (mpu_set_sample_rate(800)){ if (mpu_set_sample_rate(800)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n")); hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n");
goto failed; goto failed;
} }
// Select which sensors are pushed to FIFO. // Select which sensors are pushed to FIFO.
sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO; sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO;
if (mpu_configure_fifo(sensors)){ if (mpu_configure_fifo(sensors)){
hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n")); hal.console->printf("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n");
goto failed; goto failed;
} }
@ -467,18 +465,23 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
mpu_set_sensors(sensors); mpu_set_sensors(sensors);
// Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp) // Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp)
_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();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
// start the timer process to read samples // start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate)); hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate));
return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE;
failed: return true;
failed:
// give back i2c semaphore // give back i2c semaphore
i2c_sem->give(); i2c_sem->give();
return -1; return false;
} }
/** /**
@ -1017,9 +1020,9 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel,
* @brief Accumulate values from accels and gyros. * @brief Accumulate values from accels and gyros.
* *
* This method is called periodically by the scheduler. * This method is called periodically by the scheduler.
*
*/ */
void AP_InertialSensor_MPU9150::_accumulate(void){ void AP_InertialSensor_MPU9150::_accumulate(void)
{
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -1094,102 +1097,36 @@ void AP_InertialSensor_MPU9150::_accumulate(void){
_gyro_filter_y.apply(gyro_y), _gyro_filter_y.apply(gyro_y),
_gyro_filter_z.apply(gyro_z)); _gyro_filter_z.apply(gyro_z));
_gyro_samples_available++; _have_sample_available = true;
} }
// give back i2c semaphore // give back i2c semaphore
i2c_sem->give(); i2c_sem->give();
} }
bool AP_InertialSensor_MPU9150::_sample_available(void)
{
uint64_t tnow = hal.scheduler->micros();
while (tnow - _last_sample_timestamp > _sample_period_usec) {
_have_sample_available = true;
_last_sample_timestamp += _sample_period_usec;
}
return _have_sample_available;
}
bool AP_InertialSensor_MPU9150::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) {
uint64_t tnow = hal.scheduler->micros();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_period_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag));
}
if (_sample_available()) {
return true;
}
}
return false;
}
bool AP_InertialSensor_MPU9150::update(void) bool AP_InertialSensor_MPU9150::update(void)
{ {
if (!wait_for_sample(1000)) { Vector3f accel, gyro;
return false;
}
Vector3f accel_scale = _accel_scale[0].get();
_previous_accel[0] = _accel[0];
// hal.scheduler->suspend_timer_procs();
_accel[0] = _accel_filtered;
_gyro[0] = _gyro_filtered;
// 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] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available;
// 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
_gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available;
_gyro[0] -= _gyro_offset[0];
////////////////////////////////////////////////
_gyro_samples_available = 0;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_sample_available = false; _have_sample_available = false;
hal.scheduler->resume_timer_procs();
accel *= MPU9150_ACCEL_SCALE_2G;
_rotate_and_offset_accel(_accel_instance, accel);
gyro *= MPU9150_GYRO_SCALE_2000;
_rotate_and_offset_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true; return true;
} }
// TODO review to make sure it matches
float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute (a guess)
return ToRad(0.5/60);
}
// TODO review to make sure it matches
float AP_InertialSensor_MPU9150::get_delta_time(void) const
{
return _sample_period_usec * 1.0e-6f;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

30
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -12,28 +12,25 @@
#include <LowPassFilter2p.h> #include <LowPassFilter2p.h>
class AP_InertialSensor_MPU9150 : public AP_InertialSensor class AP_InertialSensor_MPU9150 : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_MPU9150(AP_InertialSensor &imu);
AP_InertialSensor_MPU9150(); /* update accel and gyro state */
/* Implementation of AP_InertialSensor functions: */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate(); bool gyro_sample_available(void) { return _have_sample_available; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return _have_sample_available; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private: private:
uint16_t _init_sensor( Sample_rate sample_rate ); bool _init_sensor();
void _accumulate(void); void _accumulate(void);
bool _sample_available();
// uint64_t _last_update_usec;
Vector3f _accel_filtered; Vector3f _accel_filtered;
Vector3f _gyro_filtered; Vector3f _gyro_filtered;
uint32_t _sample_period_usec;
volatile uint32_t _gyro_samples_available;
uint64_t _last_sample_timestamp;
bool _have_sample_available; bool _have_sample_available;
// // support for updating filter at runtime // // support for updating filter at runtime
@ -52,7 +49,6 @@ private:
int16_t mpu_set_int_latched(uint8_t enable); int16_t mpu_set_int_latched(uint8_t enable);
int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more); int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more);
// Filter (specify which one)
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
@ -62,11 +58,9 @@ private:
LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z; LowPassFilter2p _gyro_filter_z;
// LowPassFilter2p _mag_filter_x;
// LowPassFilter2p _mag_filter_y;
// LowPassFilter2p _mag_filter_z;
uint8_t _gyro_instance;
uint8_t _accel_instance;
}; };
#endif #endif
#endif // __AP_INERTIAL_SENSOR_MPU9150_H__ #endif // __AP_INERTIAL_SENSOR_MPU9150_H__

407
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -21,7 +21,6 @@
#include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_MPU9250.h"
#include "../AP_HAL_Linux/GPIO.h" #include "../AP_HAL_Linux/GPIO.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -158,29 +157,11 @@ extern const AP_HAL::HAL& hal;
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07 #define BITS_DLPF_CFG_MASK 0x07
// // TODO Remove
// // Product ID Description for MPU6000
// // high 4 bits low 4 bits
// // Product Name Product Revision
// #define MPU6000ES_REV_C4 0x14 // 0001 0100
// #define MPU6000ES_REV_C5 0x15 // 0001 0101
// #define MPU6000ES_REV_D6 0x16 // 0001 0110
// #define MPU6000ES_REV_D7 0x17 // 0001 0111
// #define MPU6000ES_REV_D8 0x18 // 0001 1000
// #define MPU6000_REV_C4 0x54 // 0101 0100
// #define MPU6000_REV_C5 0x55 // 0101 0101
// #define MPU6000_REV_D6 0x56 // 0101 0110
// #define MPU6000_REV_D7 0x57 // 0101 0111
// #define MPU6000_REV_D8 0x58 // 0101 1000
// #define MPU6000_REV_D9 0x59 // 0101 1001
/* /*
* PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of * PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
*/ */
const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); #define GYRO_SCALE (0.0174532f / 16.4f)
/* /*
* PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of * PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
@ -190,27 +171,48 @@ const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f);
* variants however * variants however
*/ */
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() : AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu),
_drdy_pin(NULL), _last_filter_hz(-1),
_initialised(false), _shared_data_idx(0),
_mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE) _accel_filter_x(1000, 15),
_accel_filter_y(1000, 15),
_accel_filter_z(1000, 15),
_gyro_filter_x(1000, 15),
_gyro_filter_y(1000, 15),
_gyro_filter_z(1000, 15),
_have_sample_available(false)
{ {
} }
uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _mpu9250_product_id;
_initialised = true;
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU9250::_init_sensor(void)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE // we need to suspend timers to prevent other SPI drivers grabbing
_drdy_pin = hal.gpio->channel(BBB_P8_7); // the bus while we do the long initialisation
_drdy_pin->mode(HAL_GPIO_INPUT);
#endif
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI); uint8_t whoami = _register_read(MPUREG_WHOAMI);
@ -218,43 +220,36 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
// TODO: we should probably accept multiple chip // TODO: we should probably accept multiple chip
// revisions. This is the one on the PXF // revisions. This is the one on the PXF
hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
hal.scheduler->panic("MPU9250: bad WHOAMI"); return false;
} }
uint8_t tries = 0; uint8_t tries = 0;
do { do {
bool success = _hardware_init(sample_rate); bool success = _hardware_init();
if (success) { if (success) {
hal.scheduler->delay(5+2); hal.scheduler->delay(10);
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); hal.console->printf("MPU9250: Unable to get semaphore");
return false;
} }
if (_data_ready()) { uint8_t status = _register_read(MPUREG_INT_STATUS);
if ((status & BIT_RAW_RDY_INT) != 0) {
_spi_sem->give(); _spi_sem->give();
break; break;
} else {
hal.console->println_P(
PSTR("MPU9250 startup failed: no data ready"));
} }
_spi_sem->give(); _spi_sem->give();
} }
if (tries++ > 5) { if (tries++ > 5) {
hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times")); return false;
} }
} while (1); } while (1);
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
/* read the first lot of data. _product_id = AP_PRODUCT_ID_MPU9250;
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
_spi_sem->give();
}
// start the timer process to read samples // start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data)); hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));
@ -262,70 +257,45 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
#if MPU9250_DEBUG #if MPU9250_DEBUG
_dump_registers(); _dump_registers();
#endif #endif
return _mpu9250_product_id; return true;
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_MPU9250::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;
} }
/*
update the accel and gyro vectors
*/
bool AP_InertialSensor_MPU9250::update( void ) bool AP_InertialSensor_MPU9250::update( void )
{ {
// wait for at least 1 sample // pull the data from the timer shared data buffer
if (!wait_for_sample(1000)) { uint8_t idx = _shared_data_idx;
return false; Vector3f gyro = _shared_data[idx]._gyro_filtered;
} Vector3f accel = _shared_data[idx]._accel_filtered;
_previous_accel[0] = _accel[0]; _have_sample_available = false;
// disable timer procs for mininum time accel *= MPU9250_ACCEL_SCALE_1G;
hal.scheduler->suspend_timer_procs(); gyro *= GYRO_SCALE;
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_sum_count = 0;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples;
// rotate for bbone default // rotate for bbone default
_accel[0].rotate(ROTATION_ROLL_180_YAW_90); accel.rotate(ROTATION_ROLL_180_YAW_90);
_gyro[0].rotate(ROTATION_ROLL_180_YAW_90); gyro.rotate(ROTATION_ROLL_180_YAW_90);
Vector3f accel_scale = _accel_scale[0].get(); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
_accel[0].x *= accel_scale.x; // PXF has an additional YAW 180
_accel[0].y *= accel_scale.y; accel.rotate(ROTATION_YAW_180);
_accel[0].z *= accel_scale.z; gyro.rotate(ROTATION_YAW_180);
_accel[0] -= _accel_offset[0]; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
// NavIO has different orientation, assuming RaspberryPi is right
// way up, and PWM pins on NavIO are at the back of the aircraft
accel.rotate(ROTATION_ROLL_180_YAW_90);
gyro.rotate(ROTATION_ROLL_180_YAW_90);
#endif
if (_last_filter_hz != _mpu6000_filter) { _rotate_and_offset_gyro(_gyro_instance, gyro);
if (_spi_sem->take(10)) { _rotate_and_offset_accel(_accel_instance, accel);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0); if (_last_filter_hz != _imu.get_filter()) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _set_filter(_imu.get_filter());
_error_count = 0; _last_filter_hz = _imu.get_filter();
_spi_sem->give();
}
} }
return true; return true;
@ -333,27 +303,11 @@ bool AP_InertialSensor_MPU9250::update( void )
/*================ HARDWARE FUNCTIONS ==================== */ /*================ HARDWARE FUNCTIONS ==================== */
/**
* Return true if the MPU9250 has new data available for reading.
*
* We use the data ready pin if it is available. Otherwise, read the
* status register.
*/
bool AP_InertialSensor_MPU9250::_data_ready()
{
if (_drdy_pin) {
return _drdy_pin->read() != 0;
}
uint8_t status = _register_read(MPUREG_INT_STATUS);
return (status & BIT_RAW_RDY_INT) != 0;
}
/** /**
* Timer process to poll for new data from the MPU9250. * Timer process to poll for new data from the MPU9250.
*/ */
void AP_InertialSensor_MPU9250::_poll_data(void) void AP_InertialSensor_MPU9250::_poll_data(void)
{ {
if (hal.scheduler->in_timerprocess()) {
if (!_spi_sem->take_nonblocking()) { if (!_spi_sem->take_nonblocking()) {
/* /*
the semaphore being busy is an expected condition when the the semaphore being busy is an expected condition when the
@ -363,29 +317,16 @@ void AP_InertialSensor_MPU9250::_poll_data(void)
*/ */
return; return;
} }
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction(); _read_data_transaction();
}
_spi_sem->give(); _spi_sem->give();
} else {
/* Synchronous read - take semaphore */
if (_spi_sem->take(10)) {
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU9250::_poll_data "
"failed to take SPI semaphore synchronously"));
}
}
} }
void AP_InertialSensor_MPU9250::_read_data_transaction() { /*
read from the data registers and update filtered data
*/
void AP_InertialSensor_MPU9250::_read_data_transaction()
{
/* one resister address followed by seven 2-byte registers */ /* one resister address followed by seven 2-byte registers */
struct PACKED { struct PACKED {
uint8_t cmd; uint8_t cmd;
@ -395,45 +336,27 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() {
_spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
if (_drdy_pin) {
if (_drdy_pin->read() != 0) {
// data ready should have gone low after a read
printf("MPU9250: DRDY didn't go low\n");
}
}
/*
detect a bad SPI bus transaction by looking for all 14 bytes
zero, or the wrong INT_STATUS register value. This is used to
detect a too high SPI bus speed.
*/
uint8_t i;
for (i=0; i<14; i++) {
if (rx.v[i] != 0) break;
}
if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
// likely a bad bus transaction
if (++_error_count > 4) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
}
}
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
_accel_sum.x += int16_val(rx.v, 1);
_accel_sum.y += int16_val(rx.v, 0);
_accel_sum.z -= int16_val(rx.v, 2);
_gyro_sum.x += int16_val(rx.v, 5);
_gyro_sum.y += int16_val(rx.v, 4);
_gyro_sum.z -= int16_val(rx.v, 6);
_sum_count++;
if (_sum_count == 0) { Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
// rollover - v unlikely _accel_filter_y.apply(int16_val(rx.v, 0)),
_accel_sum.zero(); _accel_filter_z.apply(-int16_val(rx.v, 2)));
_gyro_sum.zero();
} Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)),
_gyro_filter_y.apply(int16_val(rx.v, 4)),
_gyro_filter_z.apply(-int16_val(rx.v, 6)));
// update the shared buffer
uint8_t idx = _shared_data_idx ^ 1;
_shared_data[idx]._accel_filtered = _accel_filtered;
_shared_data[idx]._gyro_filtered = _gyro_filtered;
_shared_data_idx = idx;
_have_sample_available = true;
} }
/*
read an 8 bit register
*/
uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
{ {
uint8_t addr = reg | 0x80; // Set most significant bit uint8_t addr = reg | 0x80; // Set most significant bit
@ -444,10 +367,12 @@ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
tx[0] = addr; tx[0] = addr;
tx[1] = 0; tx[1] = 0;
_spi->transaction(tx, rx, 2); _spi->transaction(tx, rx, 2);
return rx[1]; return rx[1];
} }
/*
write an 8 bit register
*/
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
{ {
uint8_t tx[2]; uint8_t tx[2];
@ -459,42 +384,32 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
} }
/* /*
set the DLPF filter frequency. Assumes caller has taken semaphore set the accel/gyro filter frequency
*/ */
void AP_InertialSensor_MPU9250::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
{ {
uint8_t filter = default_filter; if (filter_hz == 0) {
// choose filtering frequency filter_hz = _default_filter_hz;
switch (filter_hz) {
case 5:
filter = BITS_DLPF_CFG_5HZ;
break;
case 10:
filter = BITS_DLPF_CFG_10HZ;
break;
case 20:
filter = BITS_DLPF_CFG_20HZ;
break;
case 42:
filter = BITS_DLPF_CFG_42HZ;
break;
case 98:
filter = BITS_DLPF_CFG_98HZ;
break;
} }
if (filter != 0) { _accel_filter_x.set_cutoff_frequency(1000, filter_hz);
_last_filter_hz = filter_hz; _accel_filter_y.set_cutoff_frequency(1000, filter_hz);
_accel_filter_z.set_cutoff_frequency(1000, filter_hz);
_register_write(MPUREG_CONFIG, filter); _gyro_filter_x.set_cutoff_frequency(1000, filter_hz);
} _gyro_filter_y.set_cutoff_frequency(1000, filter_hz);
_gyro_filter_z.set_cutoff_frequency(1000, filter_hz);
} }
bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) /*
initialise the sensor configuration registers
*/
bool AP_InertialSensor_MPU9250::_hardware_init(void)
{ {
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); hal.console->printf("MPU9250: Unable to get semaphore");
return false;
} }
// initially run the bus at low speed // initially run the bus at low speed
@ -503,7 +418,11 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
// Chip reset // Chip reset
uint8_t tries; uint8_t tries;
for (tries = 0; tries<5; tries++) { for (tries = 0; tries<5; tries++) {
#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963
/* Prevent reseting if internal AK8963 is selected, because it may corrupt
* AK8963's initialisation. */
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
#endif
hal.scheduler->delay(100); hal.scheduler->delay(100);
// Wake up device and select GyroZ clock. Note that the // Wake up device and select GyroZ clock. Note that the
@ -527,68 +446,30 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
} }
_register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet) // Disable I2C bus (recommended on datasheet)
#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963
/* Prevent disabling if internal AK8963 is selected. If internal AK8963 is not used
* it's ok to disable I2C slaves*/
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1); #endif
uint8_t default_filter; _default_filter_hz = _default_filter();
// sample rate and filtering // used a fixed filter of 42Hz on the sensor, then filter using
// to minimise the effects of aliasing we choose a filter // the 2-pole software filter
// that is less than half of the sample rate _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
switch (sample_rate) {
case RATE_50HZ:
// this is used for plane and rover, where noise resistance is
// more important than update rate. Tests on an aerobatic plane
// show that 10Hz is fine, and makes it very noise resistant
default_filter = BITS_DLPF_CFG_10HZ;
_sample_shift = 2;
break;
case RATE_100HZ:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 1;
break;
case RATE_200HZ:
default:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 0;
break;
}
_set_filter_register(_mpu6000_filter, default_filter);
// set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
hal.scheduler->delay(1);
// set sample rate to 1kHz, and use the 2 pole filter to give the
// desired rate
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
hal.scheduler->delay(1);
// // read the product ID rev c has 1/2 the sensitivity of rev d
// _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
// //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
// if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
// (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
// // Accel scale 8g (4096 LSB/g)
// // Rev C has different scaling than rev D
// _register_write(MPUREG_ACCEL_CONFIG,1<<3);
// } else {
// // Accel scale 8g (4096 LSB/g)
// _register_write(MPUREG_ACCEL_CONFIG,2<<3);
// }
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g
_register_write(MPUREG_ACCEL_CONFIG,2<<3); _register_write(MPUREG_ACCEL_CONFIG,2<<3);
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives // configure interrupt to fire when new data arrives
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
hal.scheduler->delay(1);
// clear interrupt on any read, and hold the data ready pin high // clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt // until we clear the interrupt
@ -603,22 +484,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
return true; return true;
} }
// return the MPUXk gyro drift rate in radian/s/s
// note that this is much better than the oilpan gyros
float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
// return true if a sample is available
bool AP_InertialSensor_MPU9250::_sample_available()
{
_poll_data();
return (_sum_count >> _sample_shift) > 0;
}
#if MPU9250_DEBUG #if MPU9250_DEBUG
// dump all config registers - used for debug // dump all config registers - used for debug
void AP_InertialSensor_MPU9250::_dump_registers(void) void AP_InertialSensor_MPU9250::_dump_registers(void)
@ -636,12 +501,4 @@ void AP_InertialSensor_MPU9250::_dump_registers(void)
#endif #endif
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
float AP_InertialSensor_MPU9250::get_delta_time() const
{
// the sensor runs at 200Hz
return 0.005 * _num_samples;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

80
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -7,75 +7,75 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Progmem.h> #include <AP_Progmem.h>
#include <Filter.h>
#include <LowPassFilter2p.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
// enable debug to see a register dump on startup // enable debug to see a register dump on startup
#define MPU9250_DEBUG 0 #define MPU9250_DEBUG 0
class AP_InertialSensor_MPU9250 : public AP_InertialSensor class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_MPU9250(); AP_InertialSensor_MPU9250(AP_InertialSensor &imu);
/* Concrete implementation of AP_InertialSensor functions: */ /* update accel and gyro state */
bool update(); bool update();
float get_gyro_drift_rate();
// wait for a sample to be available, with timeout in milliseconds bool gyro_sample_available(void) { return _have_sample_available; }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return _have_sample_available; }
// get_delta_time returns the time period in seconds overwhich the sensor data was collected // detect the sensor
float get_delta_time() const; static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) const { return _error_count <= 4; }
bool get_gyro_health(uint8_t instance) const { return healthy(); }
bool get_accel_health(uint8_t instance) const { return healthy(); }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );
private: private:
AP_HAL::DigitalSource *_drdy_pin; bool _init_sensor(void);
bool _sample_available();
void _read_data_transaction(); void _read_data_transaction();
bool _data_ready(); bool _data_ready();
void _poll_data(void); void _poll_data(void);
uint8_t _register_read( uint8_t reg ); uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val ); void _register_write( uint8_t reg, uint8_t val );
bool _hardware_init(Sample_rate sample_rate); bool _hardware_init(void);
bool _sample_available();
AP_HAL::SPIDeviceDriver *_spi; AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem; AP_HAL::Semaphore *_spi_sem;
uint16_t _num_samples;
static const float _gyro_scale;
uint32_t _last_sample_time_micros;
// ensure we can't initialise twice
bool _initialised;
int16_t _mpu9250_product_id;
// how many hardware samples before we report a sample to the caller
uint8_t _sample_shift;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; int16_t _last_filter_hz;
void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); // change the filter frequency
void _set_filter(uint8_t filter_hz);
uint16_t _error_count; // This structure is used to pass data from the timer which reads
// the sensor to the main thread. The _shared_data_idx is used to
// prevent race conditions by ensuring the data is fully updated
// before being used by the consumer
struct {
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
} _shared_data[2];
volatile uint8_t _shared_data_idx;
// accumulation in timer - must be read with timer disabled // Low Pass filters for gyro and accel
// the sum of the values since last read LowPassFilter2p _accel_filter_x;
Vector3l _accel_sum; LowPassFilter2p _accel_filter_y;
Vector3l _gyro_sum; LowPassFilter2p _accel_filter_z;
volatile int16_t _sum_count; LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
public: // do we currently have a sample pending?
bool _have_sample_available;
// default filter frequency when set to zero
uint8_t _default_filter_hz;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;
#if MPU9250_DEBUG #if MPU9250_DEBUG
void _dump_registers(void); void _dump_registers(void);

170
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -1,11 +1,16 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
#include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_Oilpan.h"
#include <AP_ADC.h>
const extern AP_HAL::HAL& hal; const extern AP_HAL::HAL& hal;
// this driver assumes an AP_ADC object has been declared globally
extern AP_ADC_ADS7844 apm1_adc;
// ADC channel mappings on for the APM Oilpan // ADC channel mappings on for the APM Oilpan
// Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] =
{ 1, -1, -1, 1, -1, -1 }; { 1, -1, -1, 1, -1, -1 };
// ADC channel reading the gyro temperature
const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3;
// Maximum possible value returned by an offset-corrected sensor channel // Maximum possible value returned by an offset-corrected sensor channel
const float AP_InertialSensor_Oilpan::_adc_constraint = 900; const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
#define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f)
#define OILPAN_RAW_GYRO_OFFSET 1658.0f #define OILPAN_RAW_GYRO_OFFSET 1658.0f
#define ToRad(x) radians(x) // *pi/180
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
// 0.8mV/ADC step => 0.8/3.33 = 0.4 // 0.8mV/ADC step => 0.8/3.33 = 0.4
// Tested values : 0.4026, ?, 0.4192 // Tested values : 0.4026, ?, 0.4192
const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f); const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f);
const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f); const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f);
const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f); const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f);
/* ------ Public functions -------------------------------------------*/ /* ------ Public functions -------------------------------------------*/
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) :
AP_InertialSensor(), AP_InertialSensor_Backend(imu)
_adc(adc)
{ {
} }
uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate)
{
_adc->Init();
switch (sample_rate) {
case RATE_50HZ:
_sample_threshold = 20;
break;
case RATE_100HZ:
_sample_threshold = 10;
break;
case RATE_200HZ:
_sample_threshold = 5;
break;
}
#if defined(__AVR_ATmega1280__)
return AP_PRODUCT_ID_APM1_1280;
#else
return AP_PRODUCT_ID_APM1_2560;
#endif
}
bool AP_InertialSensor_Oilpan::update()
{
if (!wait_for_sample(100)) {
return false;
}
float adc_values[6];
Vector3f gyro_offset = _gyro_offset[0].get();
Vector3f accel_scale = _accel_scale[0].get();
Vector3f accel_offset = _accel_offset[0].get();
_delta_time_micros = _adc->Ch6(_sensors, adc_values);
_temp = _adc->Ch(_gyro_temp_ch);
_gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
_gyro[0].rotate(_board_orientation);
_gyro[0].x *= _gyro_gain_x;
_gyro[0].y *= _gyro_gain_y;
_gyro[0].z *= _gyro_gain_z;
_gyro[0] -= gyro_offset;
_previous_accel[0] = _accel[0];
_accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
_accel[0].rotate(_board_orientation);
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] *= OILPAN_ACCEL_SCALE_1G;
_accel[0] -= accel_offset;
/* /*
* X = 1619.30 to 2445.69 detect the sensor
* Y = 1609.45 to 2435.42
* Z = 1627.44 to 2434.82
*/ */
AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_Oilpan::_init_sensor(void)
{
apm1_adc.Init();
switch (_imu.get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
_sample_threshold = 20;
break;
case AP_InertialSensor::RATE_100HZ:
_sample_threshold = 10;
break;
case AP_InertialSensor::RATE_200HZ:
_sample_threshold = 5;
break;
default:
// can't do this speed
return false;
}
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_APM1_2560;
return true; return true;
} }
float AP_InertialSensor_Oilpan::get_delta_time() const { /*
return _delta_time_micros * 1.0e-6; copy data from ADC to frontend
} */
bool AP_InertialSensor_Oilpan::update()
/* ------ Private functions -------------------------------------------*/
// return the oilpan gyro drift rate in radian/s/s
float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
{ {
// 3.0 degrees/second/minute float adc_values[6];
return ToRad(3.0/60);
apm1_adc.Ch6(_sensors, adc_values);
// copy gyros to frontend
Vector3f v;
v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
_rotate_and_offset_gyro(_gyro_instance, v);
// copy accels to frontend
v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
v *= OILPAN_ACCEL_SCALE_1G;
_rotate_and_offset_accel(_accel_instance, v);
return true;
} }
// return true if a new sample is available // return true if a new sample is available
bool AP_InertialSensor_Oilpan::_sample_available() bool AP_InertialSensor_Oilpan::_sample_available() const
{ {
return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; return apm1_adc.num_samples_available(_sensors) >= _sample_threshold;
}
bool AP_InertialSensor_Oilpan::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

39
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -3,50 +3,35 @@
#ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ #ifndef __AP_INERTIAL_SENSOR_OILPAN_H__
#define __AP_INERTIAL_SENSOR_OILPAN_H__ #define __AP_INERTIAL_SENSOR_OILPAN_H__
#include <stdint.h> #include <AP_HAL.h>
#include <AP_ADC.h>
#include <AP_Math.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
class AP_InertialSensor_Oilpan : public AP_InertialSensor class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_Oilpan(AP_InertialSensor &imu);
AP_InertialSensor_Oilpan( AP_ADC * adc ); /* update accel and gyro state */
/* Concrete implementation of AP_InertialSensor functions: */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
// wait for a sample to be available, with timeout in milliseconds bool gyro_sample_available(void) { return _sample_available(); }
bool wait_for_sample(uint16_t timeout_ms); bool accel_sample_available(void) { return _sample_available(); }
protected: // detect the sensor
uint16_t _init_sensor(Sample_rate sample_rate); static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private: private:
bool _init_sensor(void);
bool _sample_available(); bool _sample_available() const;
AP_ADC * _adc;
float _temp;
uint32_t _delta_time_micros;
static const uint8_t _sensors[6]; static const uint8_t _sensors[6];
static const int8_t _sensor_signs[6]; static const int8_t _sensor_signs[6];
static const uint8_t _gyro_temp_ch;
static const float _gyro_gain_x; static const float _gyro_gain_x;
static const float _gyro_gain_y; static const float _gyro_gain_y;
static const float _gyro_gain_z; static const float _gyro_gain_z;
static const float _adc_constraint; static const float _adc_constraint;
uint8_t _sample_threshold; uint8_t _sample_threshold;
uint8_t _gyro_instance;
uint8_t _accel_instance;
}; };
#endif // __AP_INERTIAL_SENSOR_OILPAN_H__ #endif // __AP_INERTIAL_SENSOR_OILPAN_H__

227
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -1,7 +1,8 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_InertialSensor_PX4.h" #include "AP_InertialSensor_PX4.h"
const extern AP_HAL::HAL& hal; const extern AP_HAL::HAL& hal;
@ -15,11 +16,33 @@ const extern AP_HAL::HAL& hal;
#include <drivers/drv_gyro.h> #include <drivers/drv_gyro.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <AP_Notify.h> #include <stdio.h>
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_get_sample_timestamp(0)
{ {
// assumes max 2 instances }
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_PX4::_init_sensor(void)
{
// assumes max 3 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY); _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY); _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY); _accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
@ -32,45 +55,30 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_fd[i] >= 0) { if (_accel_fd[i] >= 0) {
_num_accel_instances = i+1; _num_accel_instances = i+1;
_accel_instance[i] = _imu.register_accel();
} }
if (_gyro_fd[i] >= 0) { if (_gyro_fd[i] >= 0) {
_num_gyro_instances = i+1; _num_gyro_instances = i+1;
_gyro_instance[i] = _imu.register_gyro();
} }
} }
if (_num_accel_instances == 0) { if (_num_accel_instances == 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); return false;
} }
if (_num_gyro_instances == 0) { if (_num_gyro_instances == 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); return false;
} }
switch (sample_rate) { _default_filter_hz = _default_filter();
case RATE_50HZ: _set_filter_frequency(_imu.get_filter());
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
}
_set_filter_frequency(_mpu6000_filter);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
return AP_PRODUCT_ID_PX4_V2; _product_id = AP_PRODUCT_ID_PX4_V2;
#else #else
return AP_PRODUCT_ID_PX4; _product_id = AP_PRODUCT_ID_PX4;
#endif #endif
return true;
} }
/* /*
@ -89,109 +97,39 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
} }
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// multi-device interface
bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const
{
if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
if (instance >= _num_gyro_instances) {
return false;
}
if ((_last_get_sample_timestamp - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) {
// gyros have not updated
return false;
}
return true;
}
uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
{
return _num_gyro_instances;
}
bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const
{
if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
if (k >= _num_accel_instances) {
return false;
}
if ((_last_get_sample_timestamp - _last_accel_timestamp[k]) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 &&
(_previous_accel[k] - _accel[k]).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
}
return true;
}
uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
{
return _num_accel_instances;
}
bool AP_InertialSensor_PX4::update(void) bool AP_InertialSensor_PX4::update(void)
{ {
if (!wait_for_sample(100)) {
return false;
}
// get the latest sample from the sensor drivers // get the latest sample from the sensor drivers
_get_sample(); _get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) { for (uint8_t k=0; k<_num_accel_instances; k++) {
_previous_accel[k] = _accel[k]; Vector3f accel = _accel_in[k];
_accel[k] = _accel_in[k]; // calling _rotate_and_offset_accel sets the sensor healthy,
_accel[k].rotate(_board_orientation); // so we only want to do this if we have new data from it
_accel[k].x *= _accel_scale[k].get().x; if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_accel[k].y *= _accel_scale[k].get().y; _rotate_and_offset_accel(_accel_instance[k], accel);
_accel[k].z *= _accel_scale[k].get().z; _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
_accel[k] -= _accel_offset[k]; }
} }
for (uint8_t k=0; k<_num_gyro_instances; k++) { for (uint8_t k=0; k<_num_gyro_instances; k++) {
_gyro[k] = _gyro_in[k]; Vector3f gyro = _gyro_in[k];
_gyro[k].rotate(_board_orientation); // calling _rotate_and_offset_accel sets the sensor healthy,
_gyro[k] -= _gyro_offset[k]; // so we only want to do this if we have new data from it
if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
_rotate_and_offset_gyro(_gyro_instance[k], gyro);
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
}
} }
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();
} }
_have_sample_available = false;
return true; return true;
} }
float AP_InertialSensor_PX4::get_delta_time(void) const
{
return _sample_time_usec * 1.0e-6f;
}
float AP_InertialSensor_PX4::get_gyro_drift_rate(void)
{
// assume 0.5 degrees/second/minute
return ToRad(0.5/60);
}
void AP_InertialSensor_PX4::_get_sample(void) void AP_InertialSensor_PX4::_get_sample(void)
{ {
for (uint8_t i=0; i<_num_accel_instances; i++) { for (uint8_t i=0; i<_num_accel_instances; i++) {
@ -201,6 +139,7 @@ void AP_InertialSensor_PX4::_get_sample(void)
accel_report.timestamp != _last_accel_timestamp[i]) { accel_report.timestamp != _last_accel_timestamp[i]) {
_accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z); _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp[i] = accel_report.timestamp; _last_accel_timestamp[i] = accel_report.timestamp;
_set_accel_error_count(_accel_instance[i], accel_report.error_count);
} }
} }
for (uint8_t i=0; i<_num_gyro_instances; i++) { for (uint8_t i=0; i<_num_gyro_instances; i++) {
@ -210,64 +149,32 @@ void AP_InertialSensor_PX4::_get_sample(void)
gyro_report.timestamp != _last_gyro_timestamp[i]) { gyro_report.timestamp != _last_gyro_timestamp[i]) {
_gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_last_gyro_timestamp[i] = gyro_report.timestamp; _last_gyro_timestamp[i] = gyro_report.timestamp;
_set_gyro_error_count(_gyro_instance[i], gyro_report.error_count);
} }
} }
_last_get_sample_timestamp = hrt_absolute_time(); _last_get_sample_timestamp = hal.scheduler->micros64();
} }
bool AP_InertialSensor_PX4::_sample_available(void) bool AP_InertialSensor_PX4::gyro_sample_available(void)
{ {
uint64_t tnow = hrt_absolute_time(); _get_sample();
while (tnow - _last_sample_timestamp > _sample_time_usec) { for (uint8_t i=0; i<_num_gyro_instances; i++) {
_have_sample_available = true; if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) {
_last_sample_timestamp += _sample_time_usec;
}
return _have_sample_available;
}
bool AP_InertialSensor_PX4::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) {
uint64_t tnow = hrt_absolute_time();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
}
if (_sample_available()) {
return true; return true;
} }
} }
return false; return false;
} }
/** bool AP_InertialSensor_PX4::accel_sample_available(void)
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_PX4::healthy(void) const
{
return get_gyro_health(0) && get_accel_health(0);
}
uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const
{
for (uint8_t i=0; i<_num_gyro_instances; i++) {
if (get_gyro_health(i)) return i;
}
return 0;
}
uint8_t AP_InertialSensor_PX4::get_primary_accel(void) const
{ {
_get_sample();
for (uint8_t i=0; i<_num_accel_instances; i++) { for (uint8_t i=0; i<_num_accel_instances; i++) {
if (get_accel_health(i)) return i; if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) {
return true;
} }
return 0; }
return false;
} }
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

44
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -4,7 +4,7 @@
#define __AP_INERTIAL_SENSOR_PX4_H__ #define __AP_INERTIAL_SENSOR_PX4_H__
#include <AP_HAL.h> #include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_Progmem.h> #include <AP_Progmem.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
@ -13,47 +13,33 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
class AP_InertialSensor_PX4 : public AP_InertialSensor class AP_InertialSensor_PX4 : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_PX4() : AP_InertialSensor_PX4(AP_InertialSensor &imu);
AP_InertialSensor(),
_last_get_sample_timestamp(0),
_sample_time_usec(0)
{
}
/* Concrete implementation of AP_InertialSensor functions: */ /* update accel and gyro state */
bool update(); bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
bool wait_for_sample(uint16_t timeout_ms);
bool healthy(void) const;
// multi-device interface // detect the sensor
bool get_gyro_health(uint8_t instance) const; static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
uint8_t get_gyro_count(void) const;
bool get_accel_health(uint8_t instance) const; bool gyro_sample_available(void);
uint8_t get_accel_count(void) const; bool accel_sample_available(void);
uint8_t get_primary_accel(void) const;
private: private:
uint8_t _get_primary_gyro(void) const; bool _init_sensor(void);
uint16_t _init_sensor( Sample_rate sample_rate );
void _get_sample(void); void _get_sample(void);
bool _sample_available(void); bool _sample_available(void);
Vector3f _accel_in[INS_MAX_INSTANCES]; Vector3f _accel_in[INS_MAX_INSTANCES];
Vector3f _gyro_in[INS_MAX_INSTANCES]; Vector3f _gyro_in[INS_MAX_INSTANCES];
uint64_t _last_accel_timestamp[INS_MAX_INSTANCES]; uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES]; uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES];
uint64_t _last_get_sample_timestamp; uint64_t _last_get_sample_timestamp;
uint64_t _last_sample_timestamp; uint64_t _last_sample_timestamp;
uint32_t _sample_time_usec;
bool _have_sample_available;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; uint8_t _last_filter_hz;
@ -64,8 +50,14 @@ private:
// accelerometer and gyro driver handles // accelerometer and gyro driver handles
uint8_t _num_accel_instances; uint8_t _num_accel_instances;
uint8_t _num_gyro_instances; uint8_t _num_gyro_instances;
int _accel_fd[INS_MAX_INSTANCES]; int _accel_fd[INS_MAX_INSTANCES];
int _gyro_fd[INS_MAX_INSTANCES]; int _gyro_fd[INS_MAX_INSTANCES];
// indexes in frontend object. Note that these could be different
// from the backend indexes
uint8_t _accel_instance[INS_MAX_INSTANCES];
uint8_t _gyro_instance[INS_MAX_INSTANCES];
}; };
#endif #endif // CONFIG_HAL_BOARD
#endif // __AP_INERTIAL_SENSOR_PX4_H__ #endif // __AP_INERTIAL_SENSOR_PX4_H__

24
libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
View File

@ -36,28 +36,12 @@
#include <AP_Declination.h> #include <AP_Declination.h>
#include <AP_NavEKF.h> #include <AP_NavEKF.h>
#include <AP_HAL_Linux.h> #include <AP_HAL_Linux.h>
#include <AP_Rally.h>
#include <AP_Scheduler.h>
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
#define CONFIG_INS_TYPE HAL_INS_DEFAULT AP_InertialSensor ins;
#if CONFIG_INS_TYPE == HAL_INS_MPU6000
AP_InertialSensor_MPU6000 ins;
#elif CONFIG_INS_TYPE == HAL_INS_PX4
AP_InertialSensor_PX4 ins;
#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN
AP_InertialSensor_VRBRAIN ins;
#elif CONFIG_INS_TYPE == HAL_INS_HIL
AP_InertialSensor_HIL ins;
#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE
AP_InertialSensor_Flymaple ins;
#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
#elif CONFIG_INS_TYPE == HAL_INS_MPU9250
AP_InertialSensor_MPU9250 ins;
#else
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
void setup(void) void setup(void)
{ {
@ -208,7 +192,7 @@ void run_test()
while( !hal.console->available() ) { while( !hal.console->available() ) {
// wait until we have a sample // wait until we have a sample
ins.wait_for_sample(1000); ins.wait_for_sample();
// read samples from ins // read samples from ins
ins.update(); ins.update();