mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-15 12:23:58 -04:00
AP_InertialSensor: first steps in frontend/backend split
This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
This commit is contained in:
Andrew Tridgell
parent
df16dd67d2
commit
448efc70a3
@ -219,18 +219,52 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
|
||||
AP_InertialSensor::AP_InertialSensor() :
|
||||
_accel(),
|
||||
_gyro(),
|
||||
_board_orientation(ROTATION_NONE)
|
||||
_board_orientation(ROTATION_NONE),
|
||||
_gyro_count(0),
|
||||
_accel_count(0)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
_backends[i] = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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
|
||||
AP_InertialSensor::init( Start_style style,
|
||||
Sample_rate sample_rate)
|
||||
{
|
||||
_product_id = _init_sensor(sample_rate);
|
||||
if (_gyro_count == 0 && _accel_count == 0) {
|
||||
// detect available backends. Only called once
|
||||
_detect_backends(sample_rate);
|
||||
}
|
||||
|
||||
// check scaling
|
||||
_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++) {
|
||||
if (_accel_scale[i].get().is_zero()) {
|
||||
_accel_scale[i].set(Vector3f(1,1,1));
|
||||
@ -241,6 +275,37 @@ AP_InertialSensor::init( Start_style style,
|
||||
// do cold-start calibration for gyro only
|
||||
_init_gyro();
|
||||
}
|
||||
|
||||
switch (sample_rate) {
|
||||
case RATE_50HZ:
|
||||
_delta_time = 1.0f/50;
|
||||
break;
|
||||
case RATE_100HZ:
|
||||
_delta_time = 1.0f/100;
|
||||
break;
|
||||
case RATE_200HZ:
|
||||
_delta_time = 1.0f/200;
|
||||
break;
|
||||
case RATE_400HZ:
|
||||
default:
|
||||
_delta_time = 1.0f/400;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
detect available backends for this board
|
||||
*/
|
||||
void
|
||||
AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
|
||||
{
|
||||
_backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
|
||||
if (_backends[0] == NULL ||
|
||||
_gyro_count == 0 ||
|
||||
_accel_count == 0) {
|
||||
hal.scheduler->panic(PSTR("No INS backends available"));
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
@ -325,10 +390,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
|
||||
}
|
||||
uint8_t num_samples = 0;
|
||||
while (num_samples < 32) {
|
||||
if (!wait_for_sample(1000)) {
|
||||
interact->printf_P(PSTR("Failed to get INS sample\n"));
|
||||
goto failed;
|
||||
}
|
||||
wait_for_sample();
|
||||
// read samples from ins
|
||||
update();
|
||||
// capture sample
|
||||
@ -829,3 +891,35 @@ void AP_InertialSensor::_save_parameters()
|
||||
_gyro_offset[i].save();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update gyro and accel values from backends
|
||||
*/
|
||||
void AP_InertialSensor::update(void)
|
||||
{
|
||||
for (int8_t i=INS_MAX_INSTANCES-1; i>=0; i--) {
|
||||
if (_backends[i] != NULL) {
|
||||
_backends[i]->update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
wait for a sample to be available
|
||||
*/
|
||||
void AP_InertialSensor::wait_for_sample(void)
|
||||
{
|
||||
bool gyro_available = false;
|
||||
bool accel_available = false;
|
||||
|
||||
while (!gyro_available || !accel_available) {
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
if (_backends[i] != NULL) {
|
||||
gyro_available |= _backends[i]->gyro_sample_available();
|
||||
accel_available |= _backends[i]->gyro_sample_available();
|
||||
}
|
||||
}
|
||||
hal.scheduler->delay_microseconds(100);
|
||||
}
|
||||
}
|
||||
|
||||
@ -11,9 +11,7 @@
|
||||
maximum number of INS instances available on this platform. If more
|
||||
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
|
||||
#define INS_MAX_INSTANCES 3
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
|
||||
#define INS_MAX_INSTANCES 3
|
||||
#else
|
||||
#define INS_MAX_INSTANCES 1
|
||||
@ -23,6 +21,9 @@
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Math.h>
|
||||
#include "AP_InertialSensor_UserInteract.h"
|
||||
|
||||
class AP_InertialSensor_Backend;
|
||||
|
||||
/* AP_InertialSensor is an abstraction for gyro and accel measurements
|
||||
* which are correctly aligned to the body axes and scaled to SI units.
|
||||
*
|
||||
@ -32,12 +33,11 @@
|
||||
*/
|
||||
class AP_InertialSensor
|
||||
{
|
||||
friend class AP_InertialSensor_Backend;
|
||||
|
||||
public:
|
||||
AP_InertialSensor();
|
||||
|
||||
// empty virtual destructor
|
||||
virtual ~AP_InertialSensor() {}
|
||||
|
||||
enum Start_style {
|
||||
COLD_START = 0,
|
||||
WARM_START
|
||||
@ -64,22 +64,28 @@ public:
|
||||
///
|
||||
/// @param style The initialisation startup style.
|
||||
///
|
||||
virtual void init( Start_style style,
|
||||
Sample_rate sample_rate);
|
||||
void init( Start_style style,
|
||||
Sample_rate sample_rate);
|
||||
|
||||
/// Perform cold startup initialisation for just the accelerometers.
|
||||
///
|
||||
/// @note This should not be called unless ::init has previously
|
||||
/// 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__ )
|
||||
// perform accelerometer calibration including providing user instructions
|
||||
// and feedback
|
||||
virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
|
||||
float& trim_roll,
|
||||
float& trim_pitch);
|
||||
bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
|
||||
float& trim_roll,
|
||||
float& trim_pitch);
|
||||
#endif
|
||||
|
||||
/// calibrated - returns true if the accelerometers have been calibrated
|
||||
@ -93,65 +99,63 @@ public:
|
||||
/// @note This should not be called unless ::init has previously
|
||||
/// been called, as ::init may perform other work
|
||||
///
|
||||
virtual void init_gyro(void);
|
||||
void init_gyro(void);
|
||||
|
||||
/// Fetch the current gyro values
|
||||
///
|
||||
/// @returns vector of rotational rates in radians/sec
|
||||
///
|
||||
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
|
||||
const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
|
||||
virtual void set_gyro(uint8_t instance, const Vector3f &gyro) {}
|
||||
const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
|
||||
void set_gyro(uint8_t instance, const Vector3f &gyro);
|
||||
|
||||
// set gyro offsets in radians/sec
|
||||
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
|
||||
///
|
||||
/// @returns vector of current accelerations in m/s/s
|
||||
///
|
||||
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
|
||||
const Vector3f &get_accel(void) const { return get_accel(get_primary_accel()); }
|
||||
virtual void set_accel(uint8_t instance, const Vector3f &accel) {}
|
||||
const Vector3f &get_accel(void) const { return get_accel(_primary_accel); }
|
||||
void set_accel(uint8_t instance, const Vector3f &accel);
|
||||
|
||||
// multi-device interface
|
||||
virtual bool get_gyro_health(uint8_t instance) const { return true; }
|
||||
bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
|
||||
bool get_gyro_health(uint8_t instance) const { return true; }
|
||||
bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
|
||||
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_all() const;
|
||||
|
||||
virtual bool get_accel_health(uint8_t instance) const { return true; }
|
||||
bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
|
||||
bool get_accel_health(uint8_t instance) const { return true; }
|
||||
bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
|
||||
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
|
||||
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
|
||||
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()); }
|
||||
|
||||
/* Update the sensor data, so that getters are nonblocking.
|
||||
* Returns a bool of whether data was updated or not.
|
||||
*/
|
||||
virtual bool update() = 0;
|
||||
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
|
||||
|
||||
/* get_delta_time returns the time period in seconds
|
||||
* 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
|
||||
// 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
|
||||
virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
|
||||
// update gyro and accel values from accumulated samples
|
||||
void update(void);
|
||||
|
||||
// wait for a sample to be available
|
||||
void wait_for_sample(void);
|
||||
|
||||
// class level parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
@ -169,24 +173,21 @@ public:
|
||||
}
|
||||
|
||||
// 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; }
|
||||
virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
|
||||
uint16_t error_count(void) const { return 0; }
|
||||
bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
|
||||
|
||||
virtual uint8_t get_primary_accel(void) const { return 0; }
|
||||
uint8_t get_primary_accel(void) const { return 0; }
|
||||
|
||||
protected:
|
||||
private:
|
||||
|
||||
virtual uint8_t _get_primary_gyro(void) const { return 0; }
|
||||
// load backend drivers
|
||||
void _detect_backends(Sample_rate sample_rate);
|
||||
|
||||
// sensor specific init to be overwritten by descendant classes
|
||||
virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0;
|
||||
|
||||
// no-save implementations of accel and gyro initialisation routines
|
||||
virtual void _init_accel();
|
||||
|
||||
virtual void _init_gyro();
|
||||
// accel and gyro initialisation
|
||||
void _init_accel();
|
||||
void _init_gyro();
|
||||
|
||||
#if !defined( __AVR_ATmega1280__ )
|
||||
// Calibration routines borrowed from Rolfe Schmidt
|
||||
@ -194,54 +195,63 @@ protected:
|
||||
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
|
||||
|
||||
// _calibrate_accel - perform low level accel calibration
|
||||
virtual 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]);
|
||||
virtual 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]);
|
||||
virtual void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
|
||||
bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
|
||||
void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
|
||||
void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
|
||||
void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
|
||||
void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch);
|
||||
#endif
|
||||
|
||||
// save parameters to eeprom
|
||||
void _save_parameters();
|
||||
|
||||
// Most recent accelerometer reading obtained by ::update
|
||||
// backend objects
|
||||
AP_InertialSensor_Backend *_backends[INS_MAX_INSTANCES];
|
||||
|
||||
// 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;
|
||||
|
||||
// Most recent accelerometer reading
|
||||
Vector3f _accel[INS_MAX_INSTANCES];
|
||||
|
||||
// previous accelerometer reading obtained by ::update
|
||||
Vector3f _previous_accel[INS_MAX_INSTANCES];
|
||||
|
||||
// Most recent gyro reading obtained by ::update
|
||||
// Most recent gyro reading
|
||||
Vector3f _gyro[INS_MAX_INSTANCES];
|
||||
|
||||
// timestamp of latest gyro and accel readings
|
||||
uint32_t _last_gyro_sample_time_usec[INS_MAX_INSTANCES];
|
||||
uint32_t _last_accel_sample_time_usec[INS_MAX_INSTANCES];
|
||||
|
||||
// product id
|
||||
AP_Int16 _product_id;
|
||||
|
||||
// accelerometer scaling and offsets
|
||||
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
|
||||
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
|
||||
|
||||
// filtering frequency (0 means default)
|
||||
AP_Int8 _mpu6000_filter;
|
||||
AP_Int8 _mpu6000_filter;
|
||||
|
||||
// board orientation from AHRS
|
||||
enum Rotation _board_orientation;
|
||||
enum Rotation _board_orientation;
|
||||
|
||||
// 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;
|
||||
|
||||
// time between samples
|
||||
float _delta_time;
|
||||
};
|
||||
|
||||
#include "AP_InertialSensor_Oilpan.h"
|
||||
#include "AP_InertialSensor_Backend.h"
|
||||
#include "AP_InertialSensor_MPU6000.h"
|
||||
#include "AP_InertialSensor_HIL.h"
|
||||
#include "AP_InertialSensor_PX4.h"
|
||||
#include "AP_InertialSensor_VRBRAIN.h"
|
||||
#include "AP_InertialSensor_UserInteract_Stream.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"
|
||||
#include "AP_InertialSensor_L3GD20.h"
|
||||
|
||||
#endif // __AP_INERTIAL_SENSOR_H__
|
||||
|
||||
36
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
Normal file
36
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
Normal file
@ -0,0 +1,36 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AP_InertialSensor_Backend.h"
|
||||
|
||||
AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
|
||||
_imu(imu),
|
||||
_gyro(gyro),
|
||||
_accel(accel)
|
||||
{}
|
||||
|
||||
/*
|
||||
rotate gyro vector and add the gyro offset
|
||||
*/
|
||||
void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, uint32_t now)
|
||||
{
|
||||
_imu._gyro[instance].rotate(_imu._board_orientation);
|
||||
_imu._gyro[instance] -= _imu._gyro_offset[instance];
|
||||
_imu._last_gyro_sample_time_usec[instance] = now;
|
||||
}
|
||||
|
||||
/*
|
||||
rotate accel vector, scale and add the accel offset
|
||||
*/
|
||||
void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, uint32_t now)
|
||||
{
|
||||
_imu._accel[instance].rotate(_imu._board_orientation);
|
||||
|
||||
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
|
||||
_imu._accel[instance].x *= accel_scale.x;
|
||||
_imu._accel[instance].y *= accel_scale.y;
|
||||
_imu._accel[instance].z *= accel_scale.z;
|
||||
_imu._accel[instance] -= _imu._accel_offset[instance];
|
||||
_imu._last_accel_sample_time_usec[instance] = now;
|
||||
}
|
||||
68
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Normal file
68
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Normal file
@ -0,0 +1,68 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
IMU driver backend class
|
||||
*/
|
||||
#ifndef __AP_INERTIALSENSOR_BACKEND_H__
|
||||
#define __AP_INERTIALSENSOR_BACKEND_H__
|
||||
|
||||
class AP_InertialSensor_Backend
|
||||
{
|
||||
public:
|
||||
AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
|
||||
|
||||
// we declare a virtual destructor so that drivers can
|
||||
// override with a custom destructor if need be.
|
||||
virtual ~AP_InertialSensor_Backend(void) {}
|
||||
|
||||
/*
|
||||
* Update the sensor data. Called by the frontend to transfer
|
||||
* accumulated sensor readings to the frontend structure
|
||||
*/
|
||||
virtual bool update() = 0;
|
||||
|
||||
/*
|
||||
* return true if at least one accel sample is available in the backend
|
||||
* since the last call to update()
|
||||
*/
|
||||
virtual bool accel_sample_available() = 0;
|
||||
|
||||
/*
|
||||
* return true if at least one gyro sample is available in the backend
|
||||
* since the last call to update()
|
||||
*/
|
||||
virtual bool gyro_sample_available() = 0;
|
||||
|
||||
protected:
|
||||
AP_InertialSensor &_imu; ///< access to frontend
|
||||
|
||||
// references to instance vectors
|
||||
Vector3f &_gyro;
|
||||
Vector3f &_accel;
|
||||
|
||||
// rotate gyro vector and offset
|
||||
void _rotate_and_offset_gyro(uint8_t instance, uint32_t now);
|
||||
|
||||
// rotate accel vector, scale and offset
|
||||
void _rotate_and_offset_accel(uint8_t instance, uint32_t now);
|
||||
|
||||
// note that each backend is also expected to have a detect()
|
||||
// function which instantiates an instance of the backend sensor
|
||||
// driver.
|
||||
};
|
||||
|
||||
#endif // __AP_INERTIALSENSOR_BACKEND_H__
|
||||
@ -1,5 +1,6 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#if NOT_YET
|
||||
#include "AP_InertialSensor_HIL.h"
|
||||
#include <AP_HAL.h>
|
||||
const extern AP_HAL::HAL& hal;
|
||||
@ -128,3 +129,4 @@ uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -36,6 +36,8 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#if NOT_YET
|
||||
|
||||
#include "AP_InertialSensor_L3GD20.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
@ -630,3 +632,4 @@ float AP_InertialSensor_L3GD20::get_delta_time() const
|
||||
// the sensor runs at 200Hz
|
||||
return 0.005 * _num_samples;
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#if NOT_YET
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Coded by Víctor Mayoral Vilches <v.mayoralv@gmail.com> using
|
||||
@ -826,3 +828,4 @@ float AP_InertialSensor_LSM303D::get_delta_time() const
|
||||
// the sensor runs at 200Hz
|
||||
return 0.005 * _num_samples;
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -173,26 +173,47 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
|
||||
* variants however
|
||||
*/
|
||||
|
||||
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() :
|
||||
AP_InertialSensor(),
|
||||
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
|
||||
Vector3f &gyro,
|
||||
Vector3f &accel) :
|
||||
AP_InertialSensor_Backend(imu, gyro, accel),
|
||||
_drdy_pin(NULL),
|
||||
_spi(NULL),
|
||||
_spi_sem(NULL),
|
||||
_num_samples(0),
|
||||
_last_sample_time_micros(0),
|
||||
_initialised(false),
|
||||
_mpu6000_product_id(AP_PRODUCT_ID_NONE),
|
||||
_sample_shift(0),
|
||||
_sample_count(0),
|
||||
_last_filter_hz(0),
|
||||
_error_count(0)
|
||||
_error_count(0),
|
||||
_sum_count(0)
|
||||
{
|
||||
_accel_sum.zero();
|
||||
_gyro_sum.zero();
|
||||
}
|
||||
|
||||
uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
|
||||
/*
|
||||
detect the sensor
|
||||
*/
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu,
|
||||
AP_InertialSensor::Sample_rate sample_rate,
|
||||
Vector3f &gyro,
|
||||
Vector3f &accel)
|
||||
{
|
||||
if (_initialised) return _mpu6000_product_id;
|
||||
_initialised = true;
|
||||
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, gyro, accel);
|
||||
if (sensor == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
if (!sensor->_init_sensor(sample_rate)) {
|
||||
delete sensor;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
/*
|
||||
initialise the sensor
|
||||
*/
|
||||
bool AP_InertialSensor_MPU6000::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
|
||||
{
|
||||
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
|
||||
_spi_sem = _spi->get_semaphore();
|
||||
|
||||
@ -209,99 +230,73 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
|
||||
if (success) {
|
||||
hal.scheduler->delay(5+2);
|
||||
if (!_spi_sem->take(100)) {
|
||||
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
|
||||
return false;
|
||||
}
|
||||
if (_data_ready()) {
|
||||
_spi_sem->give();
|
||||
break;
|
||||
} else {
|
||||
hal.console->println_P(
|
||||
PSTR("MPU6000 startup failed: no data ready"));
|
||||
return false;
|
||||
}
|
||||
_spi_sem->give();
|
||||
}
|
||||
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);
|
||||
|
||||
// grab the used instances
|
||||
_gyro_instance = _imu.register_gyro();
|
||||
_accel_instance = _imu.register_accel();
|
||||
|
||||
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
|
||||
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data));
|
||||
|
||||
#if MPU6000_DEBUG
|
||||
_dump_registers();
|
||||
#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 )
|
||||
{
|
||||
// wait for at least 1 sample
|
||||
if (!wait_for_sample(1000)) {
|
||||
{
|
||||
if (_sum_count < _sample_count) {
|
||||
// we don't have enough samples yet
|
||||
return false;
|
||||
}
|
||||
|
||||
_previous_accel[0] = _accel[0];
|
||||
// we have a full set of samples
|
||||
uint16_t num_samples;
|
||||
uint32_t now = hal.scheduler->micros();
|
||||
|
||||
// disable timer procs for mininum time
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
_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;
|
||||
_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();
|
||||
_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];
|
||||
_gyro *= _gyro_scale / num_samples;
|
||||
_rotate_and_offset_gyro(_gyro_instance, now);
|
||||
|
||||
_accel[0].rotate(_board_orientation);
|
||||
_accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples;
|
||||
_accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
|
||||
_rotate_and_offset_accel(_accel_instance, now);
|
||||
|
||||
Vector3f accel_scale = _accel_scale[0].get();
|
||||
_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 (_last_filter_hz != _imu.get_filter()) {
|
||||
if (_spi_sem->take(10)) {
|
||||
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
||||
_set_filter_register(_mpu6000_filter, 0);
|
||||
_set_filter_register(_imu.get_filter(), 0);
|
||||
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
|
||||
_error_count = 0;
|
||||
_spi_sem->give();
|
||||
}
|
||||
}
|
||||
@ -331,35 +326,13 @@ bool AP_InertialSensor_MPU6000::_data_ready()
|
||||
*/
|
||||
void AP_InertialSensor_MPU6000::_poll_data(void)
|
||||
{
|
||||
if (hal.scheduler->in_timerprocess()) {
|
||||
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;
|
||||
}
|
||||
if (_data_ready()) {
|
||||
_last_sample_time_micros = hal.scheduler->micros();
|
||||
_read_data_transaction();
|
||||
}
|
||||
_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"));
|
||||
}
|
||||
if (!_spi_sem->take_nonblocking()) {
|
||||
return;
|
||||
}
|
||||
if (_data_ready()) {
|
||||
_read_data_transaction();
|
||||
}
|
||||
_spi_sem->give();
|
||||
}
|
||||
|
||||
|
||||
@ -477,7 +450,7 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t
|
||||
}
|
||||
|
||||
|
||||
bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
|
||||
bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sample_rate)
|
||||
{
|
||||
if (!_spi_sem->take(100)) {
|
||||
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
|
||||
@ -525,25 +498,25 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
|
||||
// to minimise the effects of aliasing we choose a filter
|
||||
// that is less than half of the sample rate
|
||||
switch (sample_rate) {
|
||||
case RATE_50HZ:
|
||||
case AP_InertialSensor::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;
|
||||
_sample_count = 4;
|
||||
break;
|
||||
case RATE_100HZ:
|
||||
case AP_InertialSensor::RATE_100HZ:
|
||||
default_filter = BITS_DLPF_CFG_20HZ;
|
||||
_sample_shift = 1;
|
||||
_sample_count = 2;
|
||||
break;
|
||||
case RATE_200HZ:
|
||||
case AP_InertialSensor::RATE_200HZ:
|
||||
default:
|
||||
default_filter = BITS_DLPF_CFG_20HZ;
|
||||
_sample_shift = 0;
|
||||
_sample_count = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
_set_filter_register(_mpu6000_filter, default_filter);
|
||||
_set_filter_register(_imu.get_filter(), default_filter);
|
||||
|
||||
// set sample rate to 200Hz, and use _sample_divider to give
|
||||
// the requested rate to the application
|
||||
@ -554,11 +527,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
|
||||
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);
|
||||
uint8_t 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)) {
|
||||
if ((product_id == MPU6000ES_REV_C4) ||
|
||||
(product_id == MPU6000ES_REV_C5) ||
|
||||
(product_id == MPU6000_REV_C4) ||
|
||||
(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);
|
||||
@ -585,22 +560,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
|
||||
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
|
||||
// dump all config registers - used for debug
|
||||
void AP_InertialSensor_MPU6000::_dump_registers(void)
|
||||
@ -619,11 +578,3 @@ void AP_InertialSensor_MPU6000::_dump_registers(void)
|
||||
}
|
||||
}
|
||||
#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;
|
||||
}
|
||||
|
||||
@ -12,33 +12,34 @@
|
||||
// enable debug to see a register dump on startup
|
||||
#define MPU6000_DEBUG 0
|
||||
|
||||
class AP_InertialSensor_MPU6000 : public AP_InertialSensor
|
||||
class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
|
||||
{
|
||||
public:
|
||||
AP_InertialSensor_MPU6000(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
|
||||
|
||||
AP_InertialSensor_MPU6000();
|
||||
/* update accel and gyro state */
|
||||
bool update();
|
||||
|
||||
/* Concrete implementation of AP_InertialSensor functions: */
|
||||
bool update();
|
||||
float get_gyro_drift_rate();
|
||||
|
||||
// wait for a sample to be available, with timeout in milliseconds
|
||||
bool wait_for_sample(uint16_t timeout_ms);
|
||||
|
||||
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
|
||||
float get_delta_time() const;
|
||||
|
||||
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 );
|
||||
bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
|
||||
bool accel_sample_available(void) { return _sum_count >= _sample_count; }
|
||||
|
||||
// detect the sensor
|
||||
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
|
||||
AP_InertialSensor::Sample_rate sample_rate,
|
||||
Vector3f &gyro,
|
||||
Vector3f &accel);
|
||||
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;
|
||||
|
||||
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
|
||||
bool _sample_available();
|
||||
void _read_data_transaction();
|
||||
bool _data_ready();
|
||||
@ -46,41 +47,29 @@ private:
|
||||
uint8_t _register_read( uint8_t reg );
|
||||
void _register_write( 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(AP_InertialSensor::Sample_rate sample_rate);
|
||||
|
||||
AP_HAL::SPIDeviceDriver *_spi;
|
||||
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 _mpu6000_product_id;
|
||||
|
||||
// how many hardware samples before we report a sample to the caller
|
||||
uint8_t _sample_shift;
|
||||
uint8_t _sample_count;
|
||||
|
||||
// support for updating filter at runtime
|
||||
uint8_t _last_filter_hz;
|
||||
|
||||
void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
|
||||
|
||||
// count of bus errors
|
||||
uint16_t _error_count;
|
||||
|
||||
// accumulation in timer - must be read with timer disabled
|
||||
// the sum of the values since last read
|
||||
Vector3l _accel_sum;
|
||||
Vector3l _gyro_sum;
|
||||
volatile int16_t _sum_count;
|
||||
|
||||
public:
|
||||
|
||||
#if MPU6000_DEBUG
|
||||
void _dump_registers(void);
|
||||
#endif
|
||||
volatile uint16_t _sum_count;
|
||||
};
|
||||
|
||||
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__
|
||||
|
||||
Loading…
Reference in New Issue
Block a user