Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: ported to AP_HAL

This commit is contained in:
Pat Hickey 2012-10-11 17:27:19 -07:00 committed by Andrew Tridgell
parent 11bf533c57
commit 4acf2c8591
12 changed files with 288 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1,10 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <FastSerial.h>
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
#include <SPI.h>
#include <AP_Common.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
#define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0)
@ -56,11 +59,9 @@ AP_InertialSensor::AP_InertialSensor()
void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate,
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler )
void (*flash_leds_cb)(bool on))
{
_product_id = _init_sensor(scheduler, sample_rate);
_product_id = _init_sensor(sample_rate);
// check scaling
Vector3f accel_scale = _accel_scale.get();
@ -71,7 +72,7 @@ AP_InertialSensor::init( Start_style style,
if (WARM_START != style) {
// do cold-start calibration for gyro only
_init_gyro(delay_cb, flash_leds_cb);
_init_gyro(flash_leds_cb);
}
}
@ -85,24 +86,24 @@ void AP_InertialSensor::_save_parameters()
}
void
AP_InertialSensor::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
AP_InertialSensor::init_gyro(void (*flash_leds_cb)(bool on))
{
_init_gyro(delay_cb, flash_leds_cb);
_init_gyro(flash_leds_cb);
// save calibration
_save_parameters();
}
void
AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
Vector3f ins_gyro;
float best_diff = 0;
// cold start
delay_cb(100);
Serial.printf_P(PSTR("Init Gyro"));
hal.scheduler->delay(100);
hal.console->printf_P(PSTR("Init Gyro"));
// remove existing gyro offsets
_gyro_offset = Vector3f(0,0,0);
@ -111,13 +112,13 @@ AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_le
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
delay_cb(20);
hal.scheduler->delay(20);
update();
ins_gyro = get_gyro();
FLASH_LEDS(false);
delay_cb(20);
hal.scheduler->delay(20);
}
// the strategy is to average 200 points over 1 second, then do it
@ -133,7 +134,7 @@ AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_le
float diff_norm;
uint8_t i;
Serial.printf_P(PSTR("*"));
hal.console->printf_P(PSTR("*"));
gyro_sum.zero();
for (i=0; i<200; i++) {
@ -145,7 +146,7 @@ AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_le
} else if (i % 40 == 0) {
FLASH_LEDS(false);
}
delay_cb(5);
hal.scheduler->delay(5);
}
gyro_avg = gyro_sum / i;
@ -171,23 +172,23 @@ AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_le
// we've kept the user waiting long enough - use the best pair we
// found so far
Serial.printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_gyro_offset = best_avg;
}
void
AP_InertialSensor::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
AP_InertialSensor::init_accel(void (*flash_leds_cb)(bool on))
{
_init_accel(delay_cb, flash_leds_cb);
_init_accel(flash_leds_cb);
// save calibration
_save_parameters();
}
void
AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
{
int8_t flashcount = 0;
Vector3f ins_accel;
@ -197,9 +198,9 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
float max_offset;
// cold start
delay_cb(100);
hal.scheduler->delay(100);
Serial.printf_P(PSTR("Init Accel"));
hal.console->printf_P(PSTR("Init Accel"));
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
@ -224,7 +225,7 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
delay_cb(20);
hal.scheduler->delay(20);
update();
ins_accel = get_accel();
@ -233,7 +234,7 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
// display some output to the user
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
hal.console->printf_P(PSTR("*"));
FLASH_LEDS(true);
}
@ -252,24 +253,25 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
delay_cb(500);
hal.scheduler->delay(500);
} while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET);
// set the global accel offsets
_accel_offset = accel_offset;
Serial.printf_P(PSTR(" "));
hal.console->printf_P(PSTR(" "));
}
#if !defined( __AVR_ATmega1280__ )
// calibrate_accel - perform accelerometer calibration including providing user instructions and feedback
// Gauss-Newton accel calibration routines borrowed from Rolfe Schmidt
// blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on),
void (*send_msg)(const prog_char_t *, ...),
void (*wait_key)(void))
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
AP_HAL::BetterStream* c)
{
Vector3f samples[6];
Vector3f new_offsets;
@ -310,16 +312,23 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
msg = PSTR("on it's back");
break;
}
send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
c->printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
wait_key();
// wait for user input
while (!c->available()) {
hal.scheduler->delay(20);
}
// clear input buffer
while( c->available() ) {
c->read();
}
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
delay_cb(1);
hal.scheduler->delay(10);
}
// read samples from ins
@ -331,7 +340,7 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
// run the calibration routine
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
send_msg(PSTR("Calibration successful\n"));
c->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
@ -340,7 +349,7 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
return true;
}
send_msg(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
c->printf_P(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
// restore original scaling and offsets
@ -354,7 +363,8 @@ bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
// accel_offsets are output from the calibration routine
// accel_scale are output from the calibration routine
// returns true if successful
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale )
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6],
Vector3f& accel_offsets, Vector3f& accel_scale )
{
int16_t i;
int16_t num_iterations = 0;
@ -419,7 +429,8 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& ac
return success;
}
void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3])
void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6],
float beta[6], float data[3])
{
int16_t j, k;
float dx, b;

72
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -3,14 +3,15 @@
#ifndef __AP_INERTIAL_SENSOR_H__
#define __AP_INERTIAL_SENSOR_H__
#include "../AP_Math/AP_Math.h"
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#define GRAVITY 9.80665
// Gyro and Accelerometer calibration criteria
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0
#include <stdint.h>
#include <AP_HAL.h>
#include <AP_Math.h>
/* AP_InertialSensor is an abstraction for gyro and accel measurements
* which are correctly aligned to the body axes and scaled to SI units.
*
@ -48,25 +49,22 @@ public:
///
/// @param style The initialisation startup style.
///
virtual void init( Start_style style,
Sample_rate sample_rate,
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler );
virtual void init( Start_style style,
Sample_rate sample_rate,
void (*flash_leds_cb)(bool on));
/// 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 (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on));
virtual void init_accel(void (*flash_leds_cb)(bool on));
#if !defined( __AVR_ATmega1280__ )
// perform accelerometer calibration including providing user instructions and feedback
virtual bool calibrate_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
void (*send_msg)(const prog_char_t *, ...),
void (*wait_key)(void));
// perform accelerometer calibration including providing user instructions
// and feedback
virtual bool calibrate_accel(void (*flash_leds_cb)(bool on),
AP_HAL::BetterStream *c);
#endif
/// Perform cold-start initialisation for just the gyros.
@ -74,7 +72,7 @@ public:
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work
///
virtual void init_gyro(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
virtual void init_gyro(void (*flash_leds_cb)(bool on));
/// Fetch the current gyro values
///
@ -84,8 +82,8 @@ public:
void set_gyro(Vector3f gyro) { _gyro = gyro; }
// set gyro offsets in radians/sec
Vector3f get_gyro_offsets(void) { return _gyro_offset; }
void set_gyro_offsets(Vector3f offsets) { _gyro_offset.set(offsets); }
Vector3f get_gyro_offsets(void) { return _gyro_offset; }
void set_gyro_offsets(Vector3f offsets) { _gyro_offset.set(offsets); }
/// Fetch the current accelerometer values
///
@ -95,38 +93,35 @@ public:
void set_accel(Vector3f accel) { _accel = accel; }
// get accel offsets in m/s/s
Vector3f get_accel_offsets() { return _accel_offset; }
void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
Vector3f get_accel_offsets() { return _accel_offset; }
void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
// get accel scale
Vector3f get_accel_scale() { return _accel_scale; }
Vector3f get_accel_scale() { return _accel_scale; }
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not.
*/
virtual bool update() = 0;
virtual bool update() = 0;
// check if the sensors have new data
virtual bool new_data_available(void) = 0;
virtual bool new_data_available(void) = 0;
/* Temperature, in degrees celsius, of the gyro. */
virtual float temperature() = 0;
virtual float temperature() = 0;
/* get_delta_time returns the time period in seconds
* overwhich the sensor data was collected
*/
virtual float get_delta_time() { return (float)get_delta_time_micros() * 1.0e-6; }
virtual uint32_t get_delta_time_micros() = 0;
// get_last_sample_time_micros returns the time in microseconds that the last sample was taken
//virtual uint32_t get_last_sample_time_micros() = 0;
virtual float get_delta_time() { return (float)get_delta_time_micros() * 1.0e-6; }
virtual uint32_t get_delta_time_micros() = 0;
// 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;
virtual float get_gyro_drift_rate(void) = 0;
// get number of samples read from the sensors
virtual uint16_t num_samples_available() = 0;
virtual uint16_t num_samples_available() = 0;
// class level parameters
static const struct AP_Param::GroupInfo var_info[];
@ -134,13 +129,12 @@ public:
protected:
// sensor specific init to be overwritten by descendant classes
virtual uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) = 0;
virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0;
// no-save implementations of accel and gyro initialisation routines
virtual void _init_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL);
virtual void _init_gyro(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL);
virtual void _init_accel(void (*flash_leds_cb)(bool on) = NULL);
virtual void _init_gyro(void (*flash_leds_cb)(bool on) = NULL);
#if !defined( __AVR_ATmega1280__ )
// Calibration routines borrowed from Rolfe Schmidt
@ -155,16 +149,16 @@ protected:
#endif
// save parameters to eeprom
void _save_parameters();
void _save_parameters();
// Most recent accelerometer reading obtained by ::update
Vector3f _accel;
Vector3f _accel;
// Most recent gyro reading obtained by ::update
Vector3f _gyro;
Vector3f _gyro;
// product id
AP_Int16 _product_id;
AP_Int16 _product_id;
// accelerometer scaling and offsets
AP_Vector3f _accel_scale;

211
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -1,14 +1,11 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <FastSerial.h>
#include <avr/interrupt.h>
#include <AP_HAL.h>
#include "AP_InertialSensor_MPU6000.h"
#include <SPI.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <wiring.h>
#endif
extern const AP_HAL::HAL& hal;
// MPU6000 accelerometer scaling
#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0)
@ -99,7 +96,7 @@
# define BIT_USER_CTRL_I2C_MST_RESET 0x02 // reset I2C Master (only applicable if I2C_MST_EN bit is set)
# define BIT_USER_CTRL_FIFO_RESET 0x04 // Reset (i.e. clear) FIFO buffer
# define BIT_USER_CTRL_DMP_RESET 0x08 // Reset DMP
# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable SPI interface
# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
# define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
# define BIT_USER_CTRL_FIFO_EN 0x40 // Enable FIFO operations
# define BIT_USER_CTRL_DMP_EN 0x80 // Enable DMP operations
@ -160,9 +157,12 @@
#define MPU6000_50HZ 0x03
// DMP FIFO constants
#define FIFO_PACKET_SIZE 18 // Default quaternion FIFO size (4*4) + Footer(2)
#define GYRO_BIAS_FROM_GRAVITY_RATE 4 // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
#define DEFAULT_ACCEL_FUSION_GAIN 0x80 // Default gain for accel fusion (with gyros)
// Default quaternion FIFO size (4*4) + Footer(2)
#define FIFO_PACKET_SIZE 18
// Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
#define GYRO_BIAS_FROM_GRAVITY_RATE 4
// Default gain for accel fusion (with gyros)
#define DEFAULT_ACCEL_FUSION_GAIN 0x80
/*
* RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
@ -183,17 +183,28 @@ const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE;
// variables to calculate time period over which a group of samples were collected
static volatile uint32_t _delta_time_micros = 1; // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case)
static volatile uint32_t _delta_time_start_micros = 0; // time we start collecting sample (reset on update)
static volatile uint32_t _last_sample_time_micros = 0; // time latest sample was collected
// variables to calculate time period over which a group of samples were
// collected
// time period overwhich samples were collected (initialise to non-zero
// number but will be overwritten on 2nd read in any case)
static volatile uint32_t _delta_time_micros = 1;
// time we start collecting sample (reset on update)
static volatile uint32_t _delta_time_start_micros = 0;
// time latest sample was collected
static volatile uint32_t _last_sample_time_micros = 0;
// DMP related static variables
bool AP_InertialSensor_MPU6000::_dmp_initialised = false;
uint8_t AP_InertialSensor_MPU6000::_fifoCountH; // high byte of number of elements in fifo buffer
uint8_t AP_InertialSensor_MPU6000::_fifoCountL; // low byte of number of elements in fifo buffer
Quaternion AP_InertialSensor_MPU6000::quaternion; // holds the 4 quaternions representing attitude taken directly from the DMP
AP_PeriodicProcess* AP_InertialSensor_MPU6000::_scheduler = NULL;
// high byte of number of elements in fifo buffer
uint8_t AP_InertialSensor_MPU6000::_fifoCountH;
// low byte of number of elements in fifo buffer
uint8_t AP_InertialSensor_MPU6000::_fifoCountL;
// holds the 4 quaternions representing attitude taken directly from the DMP
Quaternion AP_InertialSensor_MPU6000::quaternion;
/* Static SPI device driver */
AP_HAL::SPIDeviceDriver* AP_InertialSensor_MPU6000::_spi = NULL;
AP_HAL::Semaphore* AP_InertialSensor_MPU6000::_spi_sem = NULL;
/*
* RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
@ -210,14 +221,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
_dmp_initialised = false;
}
uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate )
uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
{
if (_initialised) return _mpu6000_product_id;
_initialised = true;
_scheduler = scheduler; // store pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
scheduler->suspend_timer();
hal.scheduler->suspend_timer_procs();
hardware_init(sample_rate);
scheduler->resume_timer();
hal.scheduler->resume_timer_procs();
return _mpu6000_product_id;
}
@ -243,8 +253,7 @@ bool AP_InertialSensor_MPU6000::update( void )
while (_count == 0) /* nop */;
// disable interrupts for mininum time
uint8_t oldSREG = SREG;
cli();
hal.scheduler->begin_atomic();
for (int i=0; i<7; i++) {
sum[i] = _sum[i];
_sum[i] = 0;
@ -255,7 +264,7 @@ bool AP_InertialSensor_MPU6000::update( void )
// record sample time
_delta_time_micros = _last_sample_time_micros - _delta_time_start_micros;
_delta_time_start_micros = _last_sample_time_micros;
SREG = oldSREG;
hal.scheduler->end_atomic();
count_scale = 1.0 / count;
@ -285,31 +294,29 @@ float AP_InertialSensor_MPU6000::temperature() {
/*================ HARDWARE FUNCTIONS ==================== */
static int16_t spi_transfer_16(void)
{
uint8_t byte_H, byte_L;
byte_H = SPI.transfer(0);
byte_L = SPI.transfer(0);
return (((int16_t)byte_H)<<8) | byte_L;
}
/*
* this is called from the data_interrupt which fires when the MPU6000 has new sensor data available
* and add it to _sum[]
* Note: it is critical that no other devices on the same SPI bus attempt to read at the same time
* to ensure this is the case, these other devices must perform their SPI reads after being
* called by the AP_TimerProcess.
* this is called from the data_interrupt which fires when the MPU6000 has new
* sensor data available and add it to _sum[] to ensure this is the case,
* these other devices must perform their spi reads after being called by the
* AP_TimerProcess.
*/
void AP_InertialSensor_MPU6000::read(uint32_t)
{
// now read the data
digitalWrite(MPU6000_CS_PIN, LOW);
byte addr = MPUREG_ACCEL_XOUT_H | 0x80;
SPI.transfer(addr);
for (uint8_t i=0; i<7; i++) {
_sum[i] += spi_transfer_16();
if (_spi_sem) {
bool has = _spi_sem->get((void*)&_spi_sem);
if (!has) return;
}
digitalWrite(MPU6000_CS_PIN, HIGH);
// now read the data
_spi->cs_assert();
uint8_t addr = MPUREG_ACCEL_XOUT_H | 0x80;
_spi->transfer(addr);
for (uint8_t i=0; i<7; i++) {
uint8_t hi = _spi->transfer(0);
uint8_t lo =_spi->transfer(0);
uint16_t b = (((int16_t)hi)<<8) | lo;
_sum[i] += b;
}
_spi->cs_release();
_count++;
if (_count == 0) {
@ -323,6 +330,10 @@ void AP_InertialSensor_MPU6000::read(uint32_t)
FIFO_getPacket();
}
}
if (_spi_sem) {
_spi_sem->release((void*)&_spi_sem);
}
}
uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg )
@ -330,57 +341,56 @@ uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg )
uint8_t return_value;
uint8_t addr = reg | 0x80; // Set most significant bit
digitalWrite(MPU6000_CS_PIN, LOW);
_spi->cs_assert();
SPI.transfer(addr);
return_value = SPI.transfer(0);
_spi->transfer(addr);
return_value = _spi->transfer(0);
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_release();
return return_value;
}
void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
{
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(reg);
SPI.transfer(val);
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_assert();
_spi->transfer(reg);
_spi->transfer(val);
_spi->cs_release();
}
// MPU6000 new data interrupt on INT6
void AP_InertialSensor_MPU6000::data_interrupt(void)
{
// record time that data was available
_last_sample_time_micros = micros();
_last_sample_time_micros = hal.scheduler->micros();
// re-enable interrupts
sei();
// queue our read process to run after any currently running timed processes complete
_scheduler->queue_process( AP_InertialSensor_MPU6000::read );
hal.scheduler->defer_timer_process( AP_InertialSensor_MPU6000::read );
}
void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
{
// MPU6000 chip select setup
pinMode(MPU6000_CS_PIN, OUTPUT);
digitalWrite(MPU6000_CS_PIN, HIGH);
delay(1);
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
_spi_sem = _spi->get_semaphore();
// Chip reset
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
delay(100);
hal.scheduler->delay(100);
// Wake up device and select GyroZ clock (better performance)
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
delay(1);
hal.scheduler->delay(1);
register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
delay(1);
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
delay(1);
hal.scheduler->delay(1);
uint8_t rate, filter, default_filter;
@ -429,14 +439,14 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
// set sample rate
register_write(MPUREG_SMPLRT_DIV, rate);
delay(1);
hal.scheduler->delay(1);
// set low pass filter
register_write(MPUREG_CONFIG, filter);
delay(1);
hal.scheduler->delay(1);
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
delay(1);
hal.scheduler->delay(1);
_mpu6000_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
@ -450,14 +460,18 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
delay(1);
hal.scheduler->delay(1);
register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // configure interrupt to fire when new data arrives
delay(1);
hal.scheduler->delay(1);
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR); // clear interrupt on any read
delay(1);
hal.scheduler->delay(1);
attachInterrupt(6,data_interrupt,RISING);
if (!hal.gpio->attach_interrupt(6, data_interrupt, GPIO_RISING)) {
hal.console->println_P(
PSTR("Critical Error: AP_InertialSensor_MPU6000 could not "
"attach data ready interrupt."));
}
}
float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval )
@ -582,12 +596,13 @@ void AP_InertialSensor_MPU6000::dmp_register_write(uint8_t bank, uint8_t address
{
register_write(MPUREG_BANK_SEL,bank);
register_write(MPUREG_MEM_START_ADDR,address);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
_spi->cs_assert();
_spi->transfer(MPUREG_MEM_R_W);
for (uint8_t i=0; i<num_bytes; i++) {
SPI.transfer(data[i]);
_spi->transfer(data[i]);
}
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_release();
}
// MPU6000 DMP initialization
@ -693,12 +708,12 @@ void AP_InertialSensor_MPU6000::FIFO_getPacket()
int16_t q_long[4];
uint8_t addr = MPUREG_FIFO_R_W | 0x80; // Set most significant bit to indicate a read
uint8_t received_packet[DMP_FIFO_BUFFER_SIZE]; // FIFO packet buffer
digitalWrite(MPU6000_CS_PIN, LOW); // enable the device
SPI.transfer(addr); // send address we want to read from
_spi->cs_assert();
_spi->transfer(addr); // send address we want to read from
for(i = 0; i < _fifoCountL; i++) {
received_packet[i] = SPI.transfer(0); // request value
received_packet[i] = _spi->transfer(0); // request value
}
digitalWrite(MPU6000_CS_PIN, HIGH); // disable device
_spi->cs_release();
// we are using 16 bits resolution
q_long[0] = (int16_t) ((((uint16_t) received_packet[0]) << 8) + ((uint16_t) received_packet[1]));
@ -885,13 +900,13 @@ void AP_InertialSensor_MPU6000::dmp_set_sensor_fusion_accel_gain(uint8_t gain)
//inv_key_0_96
register_write(MPUREG_BANK_SEL,0x00);
register_write(MPUREG_MEM_START_ADDR, 0x60);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
SPI.transfer(0x00);
SPI.transfer(gain); // Original : 0x80 To test: 0x40, 0x20 (too less)
SPI.transfer(0x00);
SPI.transfer(0x00);
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_assert();
_spi->transfer(MPUREG_MEM_R_W);
_spi->transfer(0x00);
_spi->transfer(gain); // Original : 0x80 To test: 0x40, 0x20 (too less)
_spi->transfer(0x00);
_spi->transfer(0x00);
_spi->cs_release();
}
// Load initial memory values into DMP memory banks
@ -903,13 +918,13 @@ void AP_InertialSensor_MPU6000::dmp_load_mem()
for(uint8_t j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
register_write(MPUREG_MEM_START_ADDR,start_addy);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
_spi->cs_assert();
_spi->transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 16; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[i][j][k]));
SPI.transfer((uint8_t) byteToSend);
_spi->transfer((uint8_t) byteToSend);
}
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_release();
}
}
@ -917,23 +932,23 @@ void AP_InertialSensor_MPU6000::dmp_load_mem()
for(uint8_t j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
register_write(MPUREG_MEM_START_ADDR,start_addy);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
_spi->cs_assert();
_spi->transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 16; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[7][j][k]));
SPI.transfer((uint8_t) byteToSend);
_spi->transfer((uint8_t) byteToSend);
}
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_release();
}
register_write(MPUREG_MEM_START_ADDR,0x80);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
_spi->cs_assert();
_spi->transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 9; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[7][8][k]));
SPI.transfer((uint8_t) byteToSend);
_spi->transfer((uint8_t) byteToSend);
}
digitalWrite(MPU6000_CS_PIN, HIGH);
_spi->cs_release();
}
// ========= DMP MEMORY ================================

13
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -3,11 +3,9 @@
#ifndef __AP_INERTIAL_SENSOR_MPU6000_H__
#define __AP_INERTIAL_SENSOR_MPU6000_H__
#include <string.h>
#include <stdint.h>
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#include "../AP_Math/AP_Math.h"
#include <AP_HAL.h>
#include <AP_Math.h>
#include "AP_InertialSensor.h"
#define MPU6000_CS_PIN 53 // APM pin connected to mpu6000's chip select pin
@ -46,7 +44,7 @@ public:
uint32_t get_delta_time_micros();
protected:
uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
uint16_t _init_sensor( Sample_rate sample_rate );
private:
@ -56,6 +54,9 @@ private:
static void register_write( uint8_t reg, uint8_t val );
void hardware_init(Sample_rate sample_rate);
static AP_HAL::SPIDeviceDriver *_spi;
static AP_HAL::Semaphore *_spi_sem;
float _temp;
float _temp_to_celsius( uint16_t );
@ -70,8 +71,6 @@ private:
static const uint8_t _temp_data_index;
static AP_PeriodicProcess* _scheduler; // pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
// ensure we can't initialise twice
bool _initialised;
static int16_t _mpu6000_product_id;

4
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -41,9 +41,9 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
{
}
uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate)
uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate)
{
_adc->Init(scheduler);
_adc->Init();
switch (sample_rate) {
case RATE_50HZ:

7
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -3,11 +3,10 @@
#ifndef __AP_INERTIAL_SENSOR_OILPAN_H__
#define __AP_INERTIAL_SENSOR_OILPAN_H__
#include <string.h>
#include <stdint.h>
#include "../AP_ADC/AP_ADC.h"
#include "../AP_Math/AP_Math.h"
#include <AP_ADC.h>
#include <AP_Math.h>
#include "AP_InertialSensor.h"
class AP_InertialSensor_Oilpan : public AP_InertialSensor
@ -28,7 +27,7 @@ public:
uint16_t num_samples_available();
protected:
uint16_t _init_sensor(AP_PeriodicProcess * scheduler, Sample_rate sample_rate);
uint16_t _init_sensor(Sample_rate sample_rate);
private:

10
libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
View File

@ -1,8 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_InertialSensor_Stub.h"
#include <AP_HAL.h>
const extern AP_HAL::HAL& hal;
uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) {
uint16_t AP_InertialSensor_Stub::_init_sensor( Sample_rate sample_rate ) {
switch (sample_rate) {
case RATE_50HZ:
_sample_period_ms = 20;
@ -15,13 +17,12 @@ uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, S
break;
}
return AP_PRODUCT_ID_NONE;
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_Stub::update( void ) {
uint32_t now = millis();
uint32_t now = hal.scheduler->millis();
_delta_time_usec = (now - _last_update_ms) * 1000;
_last_update_ms = now;
return true;
@ -45,7 +46,8 @@ float AP_InertialSensor_Stub::get_gyro_drift_rate(void) {
}
uint16_t AP_InertialSensor_Stub::num_samples_available()
{
uint16_t ret = (millis() - _last_update_ms) / _sample_period_ms;
uint16_t ret = (hal.scheduler->millis() - _last_update_ms)
/ _sample_period_ms;
return ret;
}

8
libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
View File

@ -3,10 +3,7 @@
#ifndef __AP_INERTIAL_SENSOR_STUB_H__
#define __AP_INERTIAL_SENSOR_STUB_H__
#include <string.h>
#include <stdint.h>
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
class AP_InertialSensor_Stub : public AP_InertialSensor
@ -17,6 +14,7 @@ public:
}
/* Concrete implementation of AP_InertialSensor functions: */
uint16_t init();
bool update();
bool new_data_available();
float temperature();
@ -26,7 +24,7 @@ public:
uint16_t num_samples_available();
protected:
uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_ms;
uint32_t _last_update_ms;
uint32_t _delta_time_usec;

0
libraries/AP_InertialSensor/examples/MPU6000/Arduino.h Normal file
View File

146
libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
View File

@ -1,18 +1,17 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Simple test for the AP_InertialSensor MPU6000 driver.
// Simple test for the AP_InertialSensor driver.
//
#include <FastSerial.h>
#include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <stdarg.h>
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Math.h>
#include <AP_Param.h>
#include <AP_ADC.h>
#include <AP_InertialSensor.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_Param.h>
#define APM_HARDWARE_APM1 1
#define APM_HARDWARE_APM2 2
@ -21,68 +20,80 @@
//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#define SAMPLE_UNIT 1
#define SAMPLE_UNIT 1
#define A_LED_PIN 27
#define C_LED_PIN 25
#else
#define SAMPLE_UNIT 5 // we need 5x as many samples on the oilpan
// we need 5x as many samples on the oilpan
#define SAMPLE_UNIT 5
#define A_LED_PIN 37
#define C_LED_PIN 35
#endif
FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
AP_InertialSensor_MPU6000 ins;
#else
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
AP_ADC_ADS7844 adc;
AP_InertialSensor_Oilpan ins(&adc);
#endif
static void flash_leds(bool on) {
hal.gpio->write(A_LED_PIN, on);
hal.gpio->write(C_LED_PIN, ~on);
}
void setup(void)
{
Serial.begin(115200);
Serial.println("Doing INS startup...");
hal.console->println("AP_InertialSensor startup...");
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
isr_registry.init();
scheduler.init(&isr_registry);
hal.gpio->pinMode(A_LED_PIN, GPIO_OUTPUT);
hal.gpio->pinMode(C_LED_PIN, GPIO_OUTPUT);
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
// we need to stop the barometer from holding the SPI bus
pinMode(40, OUTPUT);
digitalWrite(40, HIGH);
hal.gpio->pinMode(40, GPIO_OUTPUT);
hal.gpio->write(40, 1);
#endif
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
adc.Init(&scheduler); // APM ADC library initialization
#endif
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ,
delay, NULL, &scheduler);
NULL);
// display initial values
display_offsets_and_scaling();
hal.console->println("Complete. Reading:");
}
void loop(void)
{
int16_t user_input;
Serial.println();
Serial.println("Menu: ");
Serial.println(" c) calibrate accelerometers");
Serial.println(" d) display offsets and scaling");
Serial.println(" l) level (capture offsets from level)");
Serial.println(" t) test");
hal.console->println();
hal.console->println_P(PSTR(
"Menu:\r\n"
" c) calibrate accelerometers\r\n"
" d) display offsets and scaling\r\n"
" l) level (capture offsets from level)\r\n"
" t) test"));
// wait for user input
while( !Serial.available() ) {
delay(20);
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
// read in user input
while( Serial.available() ) {
user_input = Serial.read();
while( hal.console->available() ) {
user_input = hal.console->read();
if( user_input == 'c' || user_input == 'C' ) {
run_calibration();
@ -104,34 +115,14 @@ void loop(void)
}
}
static void setup_printf_P(const prog_char_t *fmt, ...)
{
va_list arg_list;
va_start(arg_list, fmt);
Serial.vprintf_P(fmt, arg_list);
va_end(arg_list);
}
static void setup_wait_key(void)
{
// wait for user input
while (!Serial.available()) {
delay(20);
}
// clear input buffer
while( Serial.available() ) {
Serial.read();
}
}
void run_calibration()
{
// clear off any other characters (like line feeds,etc)
while( Serial.available() ) {
Serial.read();
while( hal.console->available() ) {
hal.console->read();
}
ins.calibrate_accel(delay, NULL, setup_printf_P, setup_wait_key);
ins.calibrate_accel(NULL, hal.console);
}
void display_offsets_and_scaling()
@ -141,15 +132,18 @@ void display_offsets_and_scaling()
Vector3f gyro_offsets = ins.get_gyro_offsets();
// display results
Serial.printf_P(PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
hal.console->printf_P(
PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
Serial.printf_P(PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
hal.console->printf_P(
PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
accel_scale.x,
accel_scale.y,
accel_scale.z);
Serial.printf_P(PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
hal.console->printf_P(
PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
gyro_offsets.x,
gyro_offsets.y,
gyro_offsets.z);
@ -158,23 +152,23 @@ void display_offsets_and_scaling()
void run_level()
{
// clear off any input in the buffer
while( Serial.available() ) {
Serial.read();
while( hal.console->available() ) {
hal.console->read();
}
// display message to user
Serial.print("Place APM on a level surface and press any key..\n");
hal.console->print("Place APM on a level surface and press any key..\n");
// wait for user input
while( !Serial.available() ) {
delay(20);
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
while( Serial.available() ) {
Serial.read();
while( hal.console->available() ) {
hal.console->read();
}
// run accel level
ins.init_accel(delay, NULL);
ins.init_accel(flash_leds);
// display results
display_offsets_and_scaling();
@ -188,19 +182,19 @@ void run_test()
float length;
// flush any user input
while( Serial.available() ) {
Serial.read();
while( hal.console->available() ) {
hal.console->read();
}
// clear out any existing samples from ins
ins.update();
// loop as long as user does not press a key
while( !Serial.available() ) {
while( !hal.console->available() ) {
// wait until we have 8 samples
while( ins.num_samples_available() < 8 * SAMPLE_UNIT ) {
delay(1);
hal.scheduler->delay(1);
}
// read samples from ins
@ -212,12 +206,14 @@ void run_test()
length = sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
// display results
Serial.printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f \t Temp:%4.2f\n"),
hal.console->printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f \t Temp:%4.2f\n"),
accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z, temperature);
}
// clear user input
while( Serial.available() ) {
Serial.read();
while( hal.console->available() ) {
hal.console->read();
}
}
AP_HAL_MAIN();

0
libraries/AP_InertialSensor/examples/MPU6000/nocore.inoflag Normal file
View File

0
libraries/AP_InertialSensor/examples/MPU6000/norelax.inoflag Normal file
View File