Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: Flymaple sensors improvements

Browse Source 
Flymaple sensors are now raw sampled at 800Hz, and LowPassFilter2p is used
to filter to the desired filter cutoff frequency.
Flymaple sensor polling is done in the main thread NOT in the timer
interrupts.
This commit is contained in:
Mike McCauley 2013-09-27 15:31:28 +10:00 committed by Andrew Tridgell
parent 23429b6b9c
commit e4eef61ec3
3 changed files with 69 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -23,20 +23,30 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
#include "AP_InertialSensor_Flymaple.h"
const extern AP_HAL::HAL& hal;
Vector3f AP_InertialSensor_Flymaple::_accel_sum;
uint32_t AP_InertialSensor_Flymaple::_accel_sum_count;
Vector3f AP_InertialSensor_Flymaple::_gyro_sum;
uint32_t AP_InertialSensor_Flymaple::_gyro_sum_count;
/// 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;
volatile bool AP_InertialSensor_Flymaple::_in_accumulate;
uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
int AP_InertialSensor_Flymaple::_accel_fd;
int AP_InertialSensor_Flymaple::_gyro_fd;
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);
///////
/// Accelerometer ADXL345 register definitions
@ -70,18 +80,20 @@ int AP_InertialSensor_Flymaple::_gyro_fd;
uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
{
// Sensors are raw sampled at 800Hz.
// Here we figure the divider to get the rate that update will be called
switch (sample_rate) {
case RATE_50HZ:
_sample_divider = 4;
_sample_divider = 16;
_default_filter_hz = 10;
break;
case RATE_100HZ:
_sample_divider = 2;
_sample_divider = 8;
_default_filter_hz = 20;
break;
case RATE_200HZ:
default:
_sample_divider = 1;
_sample_divider = 4;
_default_filter_hz = 20;
break;
}
@ -103,8 +115,8 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
// In full resoution mode, the scale factor need not change
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08);
hal.scheduler->delay(5);
// Normal power, 800Hz bandwidth:
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0e);
// Normal power, 800Hz Output Data Rate, 400Hz bandwidth:
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d);
hal.scheduler->delay(5);
// Power up default is FIFO bypass mode. FIFO is not used by the chip
@ -120,14 +132,15 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x0f);
hal.scheduler->delay(1);
// 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
// This is the least amount of filtering we can configure for this device
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS, 0x18);
hal.scheduler->delay(1);
// No interrupts
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00);
hal.scheduler->delay(1);
// FIXME:
hal.gpio->pinMode(4, GPIO_OUTPUT);
// Set up the filter desired
_set_filter_frequency(_mpu6000_filter);
return AP_PRODUCT_ID_FLYMAPLE;
}
@ -137,15 +150,20 @@ hal.gpio->pinMode(4, GPIO_OUTPUT);
*/
void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0) {
if (filter_hz == 0)
filter_hz = _default_filter_hz;
}
/// TODO ...
_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);
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
while (sample_available() == false) {
@ -153,6 +171,7 @@ bool AP_InertialSensor_Flymaple::update(void)
}
Vector3f accel_scale = _accel_scale.get();
// Not really needed since Flymaple _accumulate runs in the main thread
hal.scheduler->suspend_timer_procs();
// base the time on the gyro timestamp, as that is what is
@ -160,13 +179,11 @@ bool AP_InertialSensor_Flymaple::update(void)
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_accel = _accel_sum / _accel_sum_count;
_accel_sum.zero();
_accel_sum_count = 0;
_accel = _accel_filtered;
_accel_samples = 0;
_gyro = _gyro_sum / _gyro_sum_count;
_gyro_sum.zero();
_gyro_sum_count = 0;
_gyro = _gyro_filtered;
_gyro_samples = 0;
hal.scheduler->resume_timer_procs();
@ -188,13 +205,11 @@ bool AP_InertialSensor_Flymaple::update(void)
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_gyro -= _gyro_offset;
#if 0
// whats this all about????
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
#endif
return true;
}
@ -235,11 +250,9 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
uint8_t buffer[6];
uint64_t now = hal.scheduler->micros();
// This takes about 500us at 200kHz I2C speed
if (now > (_last_accel_timestamp + 1250)
if (now > (_last_accel_timestamp + raw_sample_interval_us)
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
{
// FIXME:
hal.gpio->write(4, 1);
// 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
@ -247,24 +260,27 @@ hal.gpio->write(4, 1);
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
int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis
_accel_sum += Vector3f(x, y, z);
_accel_sum_count++;
_accel_filtered = Vector3f(_accel_filter_x.apply(x),
_accel_filter_y.apply(y),
_accel_filter_z.apply(z));
_accel_samples++;
_last_accel_timestamp = now;
// FIXME:
hal.gpio->write(4, 0);
}
// Read gyro
now = hal.scheduler->micros();
// This takes about 500us at 200kHz I2C speed
if (now > (_last_gyro_timestamp + 1250)
if (now > (_last_gyro_timestamp + raw_sample_interval_us)
&& hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
{
// See above re order of samples in buffer
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
_gyro_sum += Vector3f(x, y, z);
_gyro_sum_count++;
_gyro_filtered = Vector3f(_gyro_filter_x.apply(x),
_gyro_filter_y.apply(y),
_gyro_filter_z.apply(z));
_gyro_samples++;
_last_gyro_timestamp = now;
}
@ -275,7 +291,7 @@ hal.gpio->write(4, 0);
bool AP_InertialSensor_Flymaple::sample_available(void)
{
_accumulate();
return min(_accel_sum_count, _gyro_sum_count) > 0;
return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
}
#endif // CONFIG_HAL_BOARD

21
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -8,6 +8,8 @@
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
#include <Filter.h>
#include <LowPassFilter2p.h>
class AP_InertialSensor_Flymaple : public AP_InertialSensor
{
@ -27,10 +29,10 @@ private:
static void _accumulate(void);
uint64_t _last_update_usec;
float _delta_time;
static Vector3f _accel_sum;
static uint32_t _accel_sum_count;
static Vector3f _gyro_sum;
static uint32_t _gyro_sum_count;
static Vector3f _accel_filtered;
static uint32_t _accel_samples;
static Vector3f _gyro_filtered;
static uint32_t _gyro_samples;
static volatile bool _in_accumulate;
static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp;
@ -41,10 +43,13 @@ private:
uint8_t _default_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
// accelerometer and gyro driver handles
static int _accel_fd;
static int _gyro_fd;
// Low Pass filters for gyro and accel
static LowPassFilter2p _accel_filter_x;
static LowPassFilter2p _accel_filter_y;
static LowPassFilter2p _accel_filter_z;
static LowPassFilter2p _gyro_filter_x;
static LowPassFilter2p _gyro_filter_y;
static LowPassFilter2p _gyro_filter_z;
};
#endif
#endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__

5
libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde
View File

@ -17,6 +17,7 @@
#include <AP_InertialSensor.h>
#include <GCS_MAVLink.h>
#include <AP_Notify.h>
#include <Filter.h>
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
AP_InertialSensor_Flymaple ins;
@ -150,7 +151,7 @@ void run_test()
Vector3f accel;
Vector3f gyro;
float length;
uint8_t counter = 0;
uint32_t counter = 0;
// flush any user input
while( hal.console->available() ) {
@ -164,7 +165,7 @@ void run_test()
while( !hal.console->available() ) {
// wait until we have a sample
while (ins.num_samples_available() == 0) /* noop */ ;
while (ins.sample_available() == false) /* noop */ ;
// read samples from ins
ins.update();