mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_FLYMAPLE: Sensor reading improvements
I2C speed increased to 400kHz Accelerometer 8g max, Normal power, 800Hz bandwidth Gyros: 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
This commit is contained in:
Mike McCauley
Andrew Tridgell
parent
06e29536f1
commit
adfcdca074
|
|
@ -46,9 +46,6 @@ In other parts of the ArduPlane tree, the changes have been #ifdefed for
|
|||
Flymaple except:
|
||||
|
||||
- ArduPlane/compat.pde
|
||||
All the wiring compatibility functions have been inline-d, else get
|
||||
contamination of the namespace at link time (multiple definitions of delay()
|
||||
etc)
|
||||
|
||||
- libraries/AP_Compass/AP_Compass_HMC5843.cpp
|
||||
- libraries/AP_Compass/Compass.h
|
||||
|
|
@ -58,6 +55,8 @@ affect other platforms.
|
|||
|
||||
Some other minor edits to eliminate compiler warnings
|
||||
|
||||
These changes have now all been included in the ardupilot mainline code.
|
||||
|
||||
Resource usage
|
||||
Resources on the Flymaple board have been allocated by the HAL:
|
||||
|
||||
|
|
@ -66,11 +65,13 @@ Pins
|
|||
GPS. 3.3V only, NOT 5V tolerant
|
||||
1 AP GPS on Flymaple Serial2 Tx out. This is where you connect the GPS.
|
||||
3.3V
|
||||
5 I2C SCL. Do not use for GPIO.
|
||||
6 Receiver PPM in
|
||||
7 Console and Mavlink on Flymaple Serial1 Rx in. Also on connector
|
||||
"COM1". 5V tolerant.
|
||||
8 Console and Mavlink on Flymaple Serial1 Tx out. Also on connector
|
||||
"COM1". 3.3V
|
||||
9 I2C SDA. Do not use for GPIO
|
||||
15 3.3V board VCC analog in. Connect to 3.3V pin.
|
||||
16 Airspeed analog in (if available). 3.3V, NOT 5V tolerant.
|
||||
19 Battery current analog in (if available). 3.3V, NOT 5V tolerant.
|
||||
|
|
@ -85,9 +86,29 @@ SysTick 1000Hz normal timers
|
|||
3,4 RCOut
|
||||
8 not used by AP
|
||||
|
||||
AP I2CDriver on Flymaple uses the libmaple Wire library, configured to clock
|
||||
at about 200kHz. The maximum possible Flymaple could support speed is limited
|
||||
by the BMP085 barometer to 400kHz.
|
||||
Current version of the Wire library is unfortunately
|
||||
bit-banged. This may change in the future.
|
||||
|
||||
At 200kHz I2C speed, it takes 500us to read the 6 byte accelerometer
|
||||
buffer, and 500us to read the 6 byte gyro buffer.
|
||||
|
||||
The SerialUSB (USB connection) to Flymaple is not used by AP. It can be used for
|
||||
debugging inside AP_HAL_FLYMAPLE, using SerialUSB.println().
|
||||
|
||||
Sensor configuration
|
||||
|
||||
The sensors are configured so:
|
||||
|
||||
ADXL345 Accelerometer
|
||||
8g full scale, full resolution mode, 800Hz bandwidth, read at 1kHz sample rate
|
||||
per sec
|
||||
|
||||
ITG3205 Gyro
|
||||
2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate, read at 1kHz sample rate
|
||||
|
||||
Installation on Linux
|
||||
|
||||
Tested with:
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
using namespace AP_HAL_FLYMAPLE_NS;
|
||||
|
||||
static TwoWire twowire(5, 9); // Flymaple has non-standard SCL, SDA
|
||||
static TwoWire twowire(5, 9, SOFT_FAST); // Flymaple has non-standard SCL, SDA, speed ~200kHz
|
||||
void FLYMAPLEI2CDriver::begin()
|
||||
{
|
||||
twowire.begin();
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@
|
|||
*/
|
||||
|
||||
// Interface to the Flymaple sensors:
|
||||
// ITG3205 Gyroscope
|
||||
// ADXL345 Accelerometer
|
||||
// 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
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
|
||||
|
|
@ -39,7 +39,7 @@ int AP_InertialSensor_Flymaple::_accel_fd;
|
|||
int AP_InertialSensor_Flymaple::_gyro_fd;
|
||||
|
||||
///////
|
||||
/// Accelerometer ADXL345 definitions
|
||||
/// Accelerometer ADXL345 register definitions
|
||||
#define FLYMAPLE_ACCELEROMETER_ADDRESS 0x53
|
||||
#define FLYMAPLE_ACCELEROMETER_XL345_DEVID 0xe5
|
||||
#define FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE 0x2c
|
||||
|
|
@ -49,14 +49,25 @@ int AP_InertialSensor_Flymaple::_gyro_fd;
|
|||
#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0 0x32
|
||||
#define FLYMAPLE_ACCELEROMETER_GRAVITY 248
|
||||
|
||||
/// Gyro ITG3205 definitions
|
||||
// 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
|
||||
#define FLYMAPLE_GYRO_ADDRESS 0x68
|
||||
#define FLYMAPLE_GYRO_WHO_AM_I 0x00
|
||||
#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
|
||||
|
||||
// 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)
|
||||
|
||||
uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
|
||||
{
|
||||
switch (sample_rate) {
|
||||
|
|
@ -84,23 +95,40 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
|
|||
hal.scheduler->delay(5);
|
||||
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
|
||||
hal.scheduler->delay(5);
|
||||
// Measure mode:
|
||||
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
|
||||
hal.scheduler->delay(5);
|
||||
// Full resolution, 8g:
|
||||
// Caution, this must agree with FLYMAPLE_ACCELEROMETER_SCALE_1G
|
||||
// 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);
|
||||
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x09);
|
||||
// Normal power, 800Hz bandwidth:
|
||||
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0e);
|
||||
hal.scheduler->delay(5);
|
||||
// Power up default is FIFO bypass mode. FIFO is not used by the chip
|
||||
|
||||
/// Init the Gyro
|
||||
// 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"));
|
||||
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00);
|
||||
hal.scheduler->delay(1);
|
||||
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x07);
|
||||
// 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);
|
||||
hal.scheduler->delay(1);
|
||||
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS,0x1e);
|
||||
// 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
|
||||
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);
|
||||
|
||||
return AP_PRODUCT_ID_FLYMAPLE;
|
||||
}
|
||||
|
||||
|
|
@ -144,13 +172,20 @@ bool AP_InertialSensor_Flymaple::update(void)
|
|||
|
||||
// add offsets and rotation
|
||||
_accel.rotate(_board_orientation);
|
||||
|
||||
// Adjust for chip scaling to get m/s/s
|
||||
_accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
|
||||
|
||||
// Now the calibration scale factor
|
||||
_accel.x *= accel_scale.x;
|
||||
_accel.y *= accel_scale.y;
|
||||
_accel.z *= accel_scale.z;
|
||||
_accel -= _accel_offset;
|
||||
|
||||
_gyro.rotate(_board_orientation);
|
||||
_gyro *= (1.0 / 14.375) * (3.1415926 / 180); // ITG3200 14.375 LSB degrees/sec with FS_SEL=3
|
||||
|
||||
// Adjust for chip scaling to get radians/sec
|
||||
_gyro *= FLYMAPLE_GYRO_SCALE_R_S;
|
||||
_gyro -= _gyro_offset;
|
||||
|
||||
#if 0
|
||||
|
|
@ -175,6 +210,7 @@ uint32_t AP_InertialSensor_Flymaple::get_last_sample_time_micros(void)
|
|||
|
||||
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);
|
||||
}
|
||||
|
|
@ -187,8 +223,10 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
|
|||
_in_accumulate = true;
|
||||
|
||||
// Read accelerometer
|
||||
uint8_t buffer[8];
|
||||
if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 8, buffer) == 0)
|
||||
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
|
||||
uint8_t buffer[6];
|
||||
// This takes about 500us at 400kHz I2C speed
|
||||
if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
|
||||
{
|
||||
// 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
|
||||
|
|
@ -203,6 +241,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
|
|||
}
|
||||
|
||||
// Read gyro
|
||||
// This takes about 500us at 400kHz I2C speed
|
||||
if (hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
|
||||
{
|
||||
int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]); // chip X axis
|
||||
|
|
@ -212,15 +251,12 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
|
|||
_gyro_sum_count++;
|
||||
_last_gyro_timestamp = hal.scheduler->micros();
|
||||
}
|
||||
// FIXME:
|
||||
hal.gpio->write(4, 0);
|
||||
|
||||
_in_accumulate = false;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Flymaple::_ins_timer(uint32_t now)
|
||||
{
|
||||
_accumulate();
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_Flymaple::sample_available(void)
|
||||
{
|
||||
_accumulate();
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@ public:
|
|||
|
||||
private:
|
||||
uint16_t _init_sensor( Sample_rate sample_rate );
|
||||
static void _ins_timer(uint32_t now);
|
||||
static void _accumulate(void);
|
||||
uint64_t _last_update_usec;
|
||||
float _delta_time;
|
||||
|
|
|
|||
Loading…
Reference in New Issue