2013-09-23 00:32:19 -03:00
|
|
|
/// -*- 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/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
Flymaple port by Mike McCauley
|
|
|
|
*/
|
|
|
|
|
|
|
|
// Interface to the Flymaple sensors:
|
2013-09-25 21:15:31 -03:00
|
|
|
// ITG3205 Gyroscope http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
|
|
|
|
// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
|
2013-09-23 00:32:19 -03:00
|
|
|
|
|
|
|
#include <AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
2013-09-27 02:31:28 -03:00
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
#include "AP_InertialSensor_Flymaple.h"
|
|
|
|
|
|
|
|
const extern AP_HAL::HAL& hal;
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
/// 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;
|
2013-09-23 00:32:19 -03:00
|
|
|
volatile bool AP_InertialSensor_Flymaple::_in_accumulate;
|
|
|
|
uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
|
|
|
|
uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
|
2013-09-27 02:31:28 -03:00
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(0, 0);
|
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(0, 0);
|
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(0, 0);
|
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(0, 0);
|
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(0, 0);
|
|
|
|
LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(0, 0);
|
|
|
|
|
|
|
|
// This is how often we wish to make raw samples of the sensors in Hz
|
|
|
|
const uint32_t raw_sample_rate_hz = 800;
|
|
|
|
// And the equivalent time between samples in microseconds
|
|
|
|
const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz);
|
2013-09-23 00:32:19 -03:00
|
|
|
|
|
|
|
///////
|
2013-09-25 21:15:31 -03:00
|
|
|
/// Accelerometer ADXL345 register definitions
|
2013-09-23 00:32:19 -03:00
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADDRESS 0x53
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_XL345_DEVID 0xe5
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE 0x2c
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL 0x2d
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT 0x31
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID 0x00
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0 0x32
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_GRAVITY 248
|
|
|
|
|
2013-09-25 21:15:31 -03:00
|
|
|
// ADXL345 accelerometer scaling
|
|
|
|
// Result will be scaled to 1m/s/s
|
|
|
|
// ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312
|
|
|
|
#define FLYMAPLE_ACCELEROMETER_SCALE_M_S (GRAVITY_MSS / 256.0f)
|
|
|
|
|
|
|
|
/// Gyro ITG3205 register definitions
|
2013-09-23 00:32:19 -03:00
|
|
|
#define FLYMAPLE_GYRO_ADDRESS 0x68
|
2013-09-25 21:15:31 -03:00
|
|
|
#define FLYMAPLE_GYRO_WHO_AM_I 0x00
|
2013-09-23 00:32:19 -03:00
|
|
|
#define FLYMAPLE_GYRO_PWR_MGM 0x3e
|
|
|
|
#define FLYMAPLE_GYRO_DLPF_FS 0x16
|
|
|
|
#define FLYMAPLE_GYRO_INT_CFG 0x17
|
|
|
|
#define FLYMAPLE_GYRO_SMPLRT_DIV 0x15
|
|
|
|
#define FLYMAPLE_GYRO_GYROX_H 0x1d
|
|
|
|
|
2013-09-25 21:15:31 -03:00
|
|
|
// ITG3200 Gyroscope scaling
|
|
|
|
// ITG3200 is 14.375 LSB degrees/sec with FS_SEL=3
|
|
|
|
// Result wil be radians/sec
|
|
|
|
#define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f)
|
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
|
|
|
|
{
|
2013-09-27 02:31:28 -03:00
|
|
|
// Sensors are raw sampled at 800Hz.
|
|
|
|
// Here we figure the divider to get the rate that update will be called
|
2013-09-23 00:32:19 -03:00
|
|
|
switch (sample_rate) {
|
|
|
|
case RATE_50HZ:
|
2013-09-27 02:31:28 -03:00
|
|
|
_sample_divider = 16;
|
2013-09-23 00:32:19 -03:00
|
|
|
_default_filter_hz = 10;
|
|
|
|
break;
|
|
|
|
case RATE_100HZ:
|
2013-09-27 02:31:28 -03:00
|
|
|
_sample_divider = 8;
|
2013-09-23 00:32:19 -03:00
|
|
|
_default_filter_hz = 20;
|
|
|
|
break;
|
|
|
|
case RATE_200HZ:
|
|
|
|
default:
|
2013-09-27 02:31:28 -03:00
|
|
|
_sample_divider = 4;
|
2013-09-23 00:32:19 -03:00
|
|
|
_default_filter_hz = 20;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Init the accelerometer
|
|
|
|
uint8_t data;
|
|
|
|
hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
|
|
|
|
if (data != FLYMAPLE_ACCELEROMETER_XL345_DEVID)
|
|
|
|
hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor"));
|
|
|
|
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
|
|
|
|
hal.scheduler->delay(5);
|
|
|
|
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
|
|
|
|
hal.scheduler->delay(5);
|
2013-09-25 21:15:31 -03:00
|
|
|
// Measure mode:
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
|
|
|
|
hal.scheduler->delay(5);
|
2013-09-25 21:15:31 -03:00
|
|
|
// Full resolution, 8g:
|
|
|
|
// Caution, this must agree with FLYMAPLE_ACCELEROMETER_SCALE_1G
|
|
|
|
// In full resoution mode, the scale factor need not change
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08);
|
|
|
|
hal.scheduler->delay(5);
|
2013-09-27 02:31:28 -03:00
|
|
|
// Normal power, 800Hz Output Data Rate, 400Hz bandwidth:
|
|
|
|
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d);
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.scheduler->delay(5);
|
2013-09-25 21:15:31 -03:00
|
|
|
// Power up default is FIFO bypass mode. FIFO is not used by the chip
|
2013-09-23 00:32:19 -03:00
|
|
|
|
2013-09-25 21:15:31 -03:00
|
|
|
// Init the Gyro
|
|
|
|
// Expect to read the same as the Gyro I2C adress:
|
|
|
|
hal.i2c->readRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_WHO_AM_I, &data);
|
|
|
|
if (data != FLYMAPLE_GYRO_ADDRESS)
|
|
|
|
hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor"));
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00);
|
|
|
|
hal.scheduler->delay(1);
|
2013-09-25 21:15:31 -03:00
|
|
|
// Sample rate divider: with 8kHz internal clock (see FLYMAPLE_GYRO_DLPF_FS),
|
|
|
|
// get 500Hz sample rate, 2 samples
|
|
|
|
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x0f);
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.scheduler->delay(1);
|
2013-09-25 21:15:31 -03:00
|
|
|
// 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
|
2013-09-27 02:31:28 -03:00
|
|
|
// This is the least amount of filtering we can configure for this device
|
2013-09-25 21:15:31 -03:00
|
|
|
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS, 0x18);
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.scheduler->delay(1);
|
2013-09-25 21:15:31 -03:00
|
|
|
// No interrupts
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00);
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
// Set up the filter desired
|
|
|
|
_set_filter_frequency(_mpu6000_filter);
|
2013-09-25 21:15:31 -03:00
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
return AP_PRODUCT_ID_FLYMAPLE;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
set the filter frequency
|
|
|
|
*/
|
|
|
|
void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
|
|
|
|
{
|
2013-09-27 02:31:28 -03:00
|
|
|
if (filter_hz == 0)
|
2013-09-23 00:32:19 -03:00
|
|
|
filter_hz = _default_filter_hz;
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
_accel_filter_x.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
|
|
|
_accel_filter_y.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
|
|
|
_accel_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
|
|
|
_gyro_filter_x.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
|
|
|
_gyro_filter_y.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
|
|
|
_gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
// This takes about 20us to run
|
2013-09-23 00:32:19 -03:00
|
|
|
bool AP_InertialSensor_Flymaple::update(void)
|
|
|
|
{
|
2013-09-26 21:33:08 -03:00
|
|
|
while (sample_available() == false) {
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
|
|
|
Vector3f accel_scale = _accel_scale.get();
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
// Not really needed since Flymaple _accumulate runs in the main thread
|
2013-09-23 00:32:19 -03:00
|
|
|
hal.scheduler->suspend_timer_procs();
|
|
|
|
|
|
|
|
// base the time on the gyro timestamp, as that is what is
|
|
|
|
// multiplied by time to integrate in DCM
|
|
|
|
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
|
|
|
|
_last_update_usec = _last_gyro_timestamp;
|
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
_accel = _accel_filtered;
|
|
|
|
_accel_samples = 0;
|
2013-09-23 00:32:19 -03:00
|
|
|
|
2013-09-27 02:31:28 -03:00
|
|
|
_gyro = _gyro_filtered;
|
|
|
|
_gyro_samples = 0;
|
2013-09-23 00:32:19 -03:00
|
|
|
|
|
|
|
hal.scheduler->resume_timer_procs();
|
|
|
|
|
|
|
|
// add offsets and rotation
|
|
|
|
_accel.rotate(_board_orientation);
|
2013-09-25 21:15:31 -03:00
|
|
|
|
|
|
|
// Adjust for chip scaling to get m/s/s
|
|
|
|
_accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
|
|
|
|
|
|
|
|
// Now the calibration scale factor
|
2013-09-23 00:32:19 -03:00
|
|
|
_accel.x *= accel_scale.x;
|
|
|
|
_accel.y *= accel_scale.y;
|
|
|
|
_accel.z *= accel_scale.z;
|
|
|
|
_accel -= _accel_offset;
|
|
|
|
|
|
|
|
_gyro.rotate(_board_orientation);
|
2013-09-25 21:15:31 -03:00
|
|
|
|
|
|
|
// Adjust for chip scaling to get radians/sec
|
|
|
|
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
|
2013-09-23 00:32:19 -03:00
|
|
|
_gyro -= _gyro_offset;
|
|
|
|
|
|
|
|
if (_last_filter_hz != _mpu6000_filter) {
|
|
|
|
_set_filter_frequency(_mpu6000_filter);
|
|
|
|
_last_filter_hz = _mpu6000_filter;
|
|
|
|
}
|
2013-09-27 02:31:28 -03:00
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
float AP_InertialSensor_Flymaple::get_delta_time(void)
|
|
|
|
{
|
|
|
|
return _delta_time;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t AP_InertialSensor_Flymaple::get_last_sample_time_micros(void)
|
|
|
|
{
|
|
|
|
return _last_update_usec;
|
|
|
|
}
|
|
|
|
|
|
|
|
float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void)
|
|
|
|
{
|
2013-09-25 21:15:31 -03:00
|
|
|
// Dont really know this for the ITG-3200.
|
2013-09-23 00:32:19 -03:00
|
|
|
// 0.5 degrees/second/minute
|
|
|
|
return ToRad(0.5/60);
|
|
|
|
}
|
|
|
|
|
2013-09-26 22:42:39 -03:00
|
|
|
// This needs to get called as often as possible.
|
|
|
|
// Its job is to accumulate samples as fast as is reasonable for the accel and gyro
|
|
|
|
// sensors.
|
|
|
|
// Cant call this from within the system timers, since the long I2C reads (up to 1ms)
|
|
|
|
// with interrupts disabled breaks lots of things
|
|
|
|
// Therefore must call this as often as possible from
|
|
|
|
// within the mainline and thropttle the reads here to suit the sensors
|
2013-09-23 00:32:19 -03:00
|
|
|
void AP_InertialSensor_Flymaple::_accumulate(void)
|
|
|
|
{
|
|
|
|
if (_in_accumulate) {
|
2013-09-26 22:42:39 -03:00
|
|
|
return; // Dont be re-entrant (how can this occur?)
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
_in_accumulate = true;
|
|
|
|
|
2013-09-26 22:42:39 -03:00
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
// Read accelerometer
|
2013-09-25 21:15:31 -03:00
|
|
|
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
|
|
|
|
uint8_t buffer[6];
|
2013-09-26 22:42:39 -03:00
|
|
|
uint64_t now = hal.scheduler->micros();
|
|
|
|
// This takes about 500us at 200kHz I2C speed
|
2013-09-27 02:31:28 -03:00
|
|
|
if (now > (_last_accel_timestamp + raw_sample_interval_us)
|
2013-09-26 22:42:39 -03:00
|
|
|
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
|
2013-09-23 00:32:19 -03:00
|
|
|
{
|
2013-09-24 08:57:19 -03:00
|
|
|
// The order is a bit wierd here since the standard we have adopted for Flymaple
|
|
|
|
// sensor orientation is different to what the board designers intended
|
|
|
|
// Caution, to support alternative chip orientations on other bords, may
|
|
|
|
// need to add a chip orientation rotate
|
2013-09-24 19:56:01 -03:00
|
|
|
int16_t y = -((((int16_t)buffer[1]) << 8) | buffer[0]); // chip X axis
|
|
|
|
int16_t x = -((((int16_t)buffer[3]) << 8) | buffer[2]); // chip Y axis
|
2013-09-24 08:57:19 -03:00
|
|
|
int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis
|
2013-09-27 02:31:28 -03:00
|
|
|
_accel_filtered = Vector3f(_accel_filter_x.apply(x),
|
|
|
|
_accel_filter_y.apply(y),
|
|
|
|
_accel_filter_z.apply(z));
|
|
|
|
_accel_samples++;
|
2013-09-26 22:42:39 -03:00
|
|
|
_last_accel_timestamp = now;
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// Read gyro
|
2013-09-26 22:42:39 -03:00
|
|
|
now = hal.scheduler->micros();
|
|
|
|
// This takes about 500us at 200kHz I2C speed
|
2013-09-27 02:31:28 -03:00
|
|
|
if (now > (_last_gyro_timestamp + raw_sample_interval_us)
|
2013-09-26 22:42:39 -03:00
|
|
|
&& hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
|
2013-09-23 00:32:19 -03:00
|
|
|
{
|
2013-09-27 02:31:28 -03:00
|
|
|
// See above re order of samples in buffer
|
2013-09-24 20:21:02 -03:00
|
|
|
int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]); // chip X axis
|
|
|
|
int16_t x = -((((int16_t)buffer[2]) << 8) | buffer[3]); // chip Y axis
|
|
|
|
int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // chip Z axis
|
2013-09-27 02:31:28 -03:00
|
|
|
_gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
|
|
|
|
_gyro_filter_y.apply(y),
|
|
|
|
_gyro_filter_z.apply(z));
|
|
|
|
_gyro_samples++;
|
2013-09-26 22:42:39 -03:00
|
|
|
_last_gyro_timestamp = now;
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
_in_accumulate = false;
|
2013-09-26 22:42:39 -03:00
|
|
|
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
|
2013-09-26 21:33:08 -03:00
|
|
|
bool AP_InertialSensor_Flymaple::sample_available(void)
|
2013-09-23 00:32:19 -03:00
|
|
|
{
|
|
|
|
_accumulate();
|
2013-09-27 02:31:28 -03:00
|
|
|
return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
|
2013-09-23 00:32:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif // CONFIG_HAL_BOARD
|
|
|
|
|