diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h new file mode 100644 index 0000000000..0ff01b75ba --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -0,0 +1,61 @@ + +#ifndef __AP_INERTIAL_SENSOR_H__ +#define __AP_INERTIAL_SENSOR_H__ + +#include "../AP_PeriodicProcess/AP_PeriodicProcess.h" + +/* AP_InertialSensor is an abstraction for gyro and accel measurements + * which are correctly aligned to the body axes and scaled to SI units. + */ +class AP_InertialSensor +{ + public: + AP_InertialSensor() {} + + virtual void init( AP_PeriodicProcess * scheduler ); + + /* Update the sensor data, so that getters are nonblocking. + * Returns a bool of whether data was updated or not. + */ + virtual bool update() = 0; + + /* Getters for individual gyro axes. + * Gyros have correct coordinate frame and units (degrees per second). + */ + virtual float gx() = 0; + virtual float gy() = 0; + virtual float gz() = 0; + + /* Same data as above gyro getters, written to array as { gx, gy, gz } */ + virtual void get_gyros( float * ) = 0; + + /* Getters for individual accel axes. + * Accels have correct coordinate frame ( flat level ax, ay = 0; az = -9.81) + * and units (meters per second squared). + */ + virtual float ax() = 0; + virtual float ay() = 0; + virtual float az() = 0; + + /* Same data as above accel getters, written to array as { ax, ay, az } */ + virtual void get_accels( float * ) = 0; + + /* Same data as above accel and gyro getters, written to array as + * { gx, gy, gz, ax, ay, az } + */ + virtual void get_sensors( float * ) = 0; + + /* Temperature, in degrees celsius, of the gyro. */ + virtual float temperature() = 0; + + /* sample_time returns the delta in microseconds since the + * last call to reset_sample_time. + */ + virtual uint32_t sample_time() = 0; + virtual void reset_sample_time() = 0; +}; + +#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_MPU6000.h" + +#endif // __AP_INERTIAL_SENSOR_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp new file mode 100644 index 0000000000..ce4ed4d4ea --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -0,0 +1,298 @@ +#include + +#include "AP_InertialSensor_MPU6000.h" + +#include +#include + +// MPU 6000 registers +#define MPUREG_WHOAMI 0x75 // +#define MPUREG_SMPLRT_DIV 0x19 // +#define MPUREG_CONFIG 0x1A // +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_ACCEL_XOUT_H 0x3B // +#define MPUREG_ACCEL_XOUT_L 0x3C // +#define MPUREG_ACCEL_YOUT_H 0x3D // +#define MPUREG_ACCEL_YOUT_L 0x3E // +#define MPUREG_ACCEL_ZOUT_H 0x3F // +#define MPUREG_ACCEL_ZOUT_L 0x40 // +#define MPUREG_TEMP_OUT_H 0x41// +#define MPUREG_TEMP_OUT_L 0x42// +#define MPUREG_GYRO_XOUT_H 0x43 // +#define MPUREG_GYRO_XOUT_L 0x44 // +#define MPUREG_GYRO_YOUT_H 0x45 // +#define MPUREG_GYRO_YOUT_L 0x46 // +#define MPUREG_GYRO_ZOUT_H 0x47 // +#define MPUREG_GYRO_ZOUT_L 0x48 // +#define MPUREG_USER_CTRL 0x6A // +#define MPUREG_PWR_MGMT_1 0x6B // +#define MPUREG_PWR_MGMT_2 0x6C // + +// Configuration bits MPU 3000 and MPU 6000 (not revised)? +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 + +int AP_InertialSensor_MPU6000::_cs_pin; + +/* pch: by the data sheet, the gyro scale should be 16.4LSB per DPS + * Given the radians conversion factor (0.174532), the gyro scale factor + * is waaaay off - output values are way too sensitive. + * Previously a divisor of 128 was appropriate. + * After tridge's changes to ::read, 50.0 seems about right based + * on making some 360 deg rotations on my desk. + * This issue requires more investigation. + */ +const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532 / 16.4); +const float AP_InertialSensor_MPU6000::_accel_scale = 9.81 / 4096.0; + +/* pch: I believe the accel and gyro indicies are correct + * but somone else should please confirm. + */ +const uint8_t AP_InertialSensor_MPU6000::_gyro_data_index[3] = { 5, 4, 6 }; +const int8_t AP_InertialSensor_MPU6000::_gyro_data_sign[3] = { 1, 1, -1 }; + +const uint8_t AP_InertialSensor_MPU6000::_accel_data_index[3] = { 1, 0, 2 }; +const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 }; + +const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3; + +AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( int cs_pin ) +{ + _cs_pin = cs_pin; /* can't use initializer list, is static */ + _gyro.x = 0; + _gyro.y = 0; + _gyro.z = 0; + _accel.x = 0; + _accel.y = 0; + _accel.z = 0; + _temp = 0; +} + +void AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler ) +{ + hardware_init(); + scheduler->register_process( &AP_InertialSensor_MPU6000::read ); +} + +// accumulation in ISR - must be read with interrupts disabled +// the sum of the values since last read +static volatile int32_t _sum[7]; + +// how many values we've accumulated since last read +static volatile uint16_t _count; + + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +bool AP_InertialSensor_MPU6000::update( void ) +{ + int32_t sum[7]; + uint16_t count; + float count_scale; + + // wait for at least 1 sample + while (_count == 0) /* nop */; + + // disable interrupts for mininum time + cli(); + for (int i=0; i<7; i++) { + sum[i] = _sum[i]; + _sum[i] = 0; + } + count = _count; + _count = 0; + sei(); + + count_scale = 1.0 / count; + + _gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale; + _gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale; + _gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale; + + _accel.x = _accel_scale * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale; + _accel.y = _accel_scale * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale; + _accel.z = _accel_scale * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale; + + _temp = _temp_to_celsius(sum[_temp_data_index] * count_scale); + + return true; +} + +float AP_InertialSensor_MPU6000::gx() { return _gyro.x; } +float AP_InertialSensor_MPU6000::gy() { return _gyro.y; } +float AP_InertialSensor_MPU6000::gz() { return _gyro.z; } + +void AP_InertialSensor_MPU6000::get_gyros( float * g ) +{ + g[0] = _gyro.x; + g[1] = _gyro.y; + g[2] = _gyro.z; +} + +float AP_InertialSensor_MPU6000::ax() { return _accel.x; } +float AP_InertialSensor_MPU6000::ay() { return _accel.y; } +float AP_InertialSensor_MPU6000::az() { return _accel.z; } + +void AP_InertialSensor_MPU6000::get_accels( float * a ) +{ + a[0] = _accel.x; + a[1] = _accel.y; + a[2] = _accel.z; +} + +void AP_InertialSensor_MPU6000::get_sensors( float * sensors ) +{ + sensors[0] = _gyro.x; + sensors[1] = _gyro.y; + sensors[2] = _gyro.z; + sensors[3] = _accel.x; + sensors[4] = _accel.y; + sensors[5] = _accel.z; +} + +float AP_InertialSensor_MPU6000::temperature() { return _temp; } + +uint32_t AP_InertialSensor_MPU6000::sample_time() +{ + uint32_t us = micros(); + /* XXX rollover creates a major bug */ + uint32_t delta = us - _last_sample_micros; + reset_sample_time(); + return delta; +} + +void AP_InertialSensor_MPU6000::reset_sample_time() +{ + _last_sample_micros = micros(); +} + +/*================ 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; +} + +void AP_InertialSensor_MPU6000::read() +{ + // We start a multibyte SPI read of sensors + byte addr = MPUREG_ACCEL_XOUT_H | 0x80; // Set most significant bit + + digitalWrite(_cs_pin, LOW); + + SPI.transfer(addr); + + for (uint8_t i=0; i<7; i++) { + _sum[i] += spi_transfer_16(); + } + _count++; + if (_count == 0) { + // rollover - v unlikely + memset((void*)_sum, 0, sizeof(_sum)); + } + + digitalWrite(_cs_pin, HIGH); +} + +uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg ) +{ + uint8_t dump; + uint8_t return_value; + uint8_t addr = reg | 0x80; // Set most significant bit + + digitalWrite(_cs_pin, LOW); + + dump = SPI.transfer(addr); + return_value = SPI.transfer(0); + + digitalWrite(_cs_pin, HIGH); + + return return_value; +} + +void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val) +{ + uint8_t dump; + digitalWrite(_cs_pin, LOW); + dump = SPI.transfer(reg); + dump = SPI.transfer(val); + digitalWrite(_cs_pin, HIGH); +} + +void AP_InertialSensor_MPU6000::hardware_init() +{ + // Need to initialize SPI if it hasn't already. + SPI.begin(); + #if F_CPU == 16000000 + SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1MHz + #else + # error MPU6000 requires SPI at 1MHZ! Need appropriate SPI clock divider. + #endif + + // MPU6000 chip select setup + pinMode(_cs_pin, OUTPUT); + digitalWrite(_cs_pin, HIGH); + delay(1); + + // Chip reset + register_write(MPUREG_PWR_MGMT_1, BIT_H_RESET); + delay(100); + // Wake up device and select GyroZ clock (better performance) + register_write(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + delay(1); + // Disable I2C bus (recommended on datasheet) + register_write(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + delay(1); + // SAMPLE RATE + register_write(MPUREG_SMPLRT_DIV,0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + delay(1); + // FS & DLPF FS=2000º/s, DLPF = 42Hz (low pass filter) + register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); + delay(1); + register_write(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000º/s + delay(1); + register_write(MPUREG_ACCEL_CONFIG,0x08); // Accel scele 4g (4096LSB/g) + delay(1); + // INT CFG => Interrupt on Data Ready + register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready + delay(1); + register_write(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + delay(1); + // Oscillator set + // register_write(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); + delay(1); + +} + +float AP_InertialSensor_MPU6000::_temp_to_celsius ( uint16_t regval ) +{ + /* TODO */ + return 20.0; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h new file mode 100644 index 0000000000..91ad4a618d --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -0,0 +1,67 @@ + +#ifndef __AP_INERTIAL_SENSOR_MPU6000_H__ +#define __AP_INERTIAL_SENSOR_MPU6000_H__ + +#include +#include + +#include "../AP_PeriodicProcess/AP_PeriodicProcess.h" +#include "../AP_Math/AP_Math.h" +#include "AP_InertialSensor.h" + +class AP_InertialSensor_MPU6000 : public AP_InertialSensor +{ + public: + + AP_InertialSensor_MPU6000( int cs_pin ); + + void init( AP_PeriodicProcess * scheduler ); + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + float gx(); + float gy(); + float gz(); + void get_gyros( float * ); + float ax(); + float ay(); + float az(); + void get_accels( float * ); + void get_sensors( float * ); + float temperature(); + uint32_t sample_time(); + void reset_sample_time(); + + static void read(); + static uint8_t register_read( uint8_t reg ); + static void register_write( uint8_t reg, uint8_t val ); + static void hardware_init(); + + private: + + Vector3f _gyro; + Vector3f _accel; + float _temp; + + uint32_t _last_sample_micros; + + float _temp_to_celsius( uint16_t ); + + static const float _accel_scale; + static const float _gyro_scale; + + static const uint8_t _gyro_data_index[3]; + static const int8_t _gyro_data_sign[3]; + + static const uint8_t _accel_data_index[3]; + static const int8_t _accel_data_sign[3]; + + static const uint8_t _temp_data_index; + + static int16_t _data[7]; + + /* TODO deprecate _cs_pin */ + static int _cs_pin; +}; + +#endif // __AP_INERTIAL_SENSOR_MPU6000_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp new file mode 100644 index 0000000000..28f956b08a --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -0,0 +1,122 @@ + +#include "AP_InertialSensor_Oilpan.h" + +// ADC channel mappings on for the APM Oilpan +// Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ +const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; + +// ADC result sign adjustment for sensors. +const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = + { 1, -1, -1, 1, -1 , -1 }; + +// ADC channel reading the gyro temperature +const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3; + +// Maximum possible value returned by an offset-corrected sensor channel +const float AP_InertialSensor_Oilpan::_adc_constraint = 900; + +// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step +// ADXL335 Sensitivity(from datasheet) => 330mV/g, +// 0.8mV/ADC step => 330/0.8 = 412 +// Tested value : 418 +// 1G in the raw data coming from the accelerometer +// Value based on actual sample data from 20 boards +const float AP_InertialSensor_Oilpan::_gravity = 423.8; + +///< would like to use _gravity here, but cannot +const float AP_InertialSensor_Oilpan::_accel_scale = 9.80665 / 423.8; + +#define ToRad(x) (x*0.01745329252) // *pi/180 +// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, +// 0.8mV/ADC step => 0.8/3.33 = 0.4 +// Tested values : 0.4026, ?, 0.4192 +const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4); +const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41); +const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41); + +/* ------ Public functions -------------------------------------------*/ + +AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : + _adc(adc) +{ + _gyro.x = 0; + _gyro.y = 0; + _gyro.z = 0; + _accel.x = 0; + _accel.y = 0; + _accel.z = 0; +} + +void AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler) +{ + _adc->Init(scheduler); +} + +bool AP_InertialSensor_Oilpan::update() +{ + uint16_t adc_values[6]; + + _sample_time = _adc->Ch6(_sensors, adc_values); + _temp = _adc->Ch(_gyro_temp_ch); + + _gyro.x = _gyro_gain_x * _sensor_signs[0] * _gyro_apply_std_offset( adc_values[0] ); + _gyro.y = _gyro_gain_y * _sensor_signs[1] * _gyro_apply_std_offset( adc_values[1] ); + _gyro.z = _gyro_gain_z * _sensor_signs[2] * _gyro_apply_std_offset( adc_values[2] ); + + _accel.x = _accel_scale * _sensor_signs[3] * _accel_apply_std_offset( adc_values[3] ); + _accel.y = _accel_scale * _sensor_signs[4] * _accel_apply_std_offset( adc_values[4] ); + _accel.z = _accel_scale * _sensor_signs[5] * _accel_apply_std_offset( adc_values[5] ); + + return true; +} + +float AP_InertialSensor_Oilpan::gx() { return _gyro.x; } +float AP_InertialSensor_Oilpan::gy() { return _gyro.y; } +float AP_InertialSensor_Oilpan::gz() { return _gyro.z; } + +void AP_InertialSensor_Oilpan::get_gyros( float * g ) +{ + g[0] = _gyro.x; + g[1] = _gyro.y; + g[2] = _gyro.z; +} + +float AP_InertialSensor_Oilpan::ax() { return _accel.x; } +float AP_InertialSensor_Oilpan::ay() { return _accel.y; } +float AP_InertialSensor_Oilpan::az() { return _accel.z; } + +void AP_InertialSensor_Oilpan::get_accels( float * a ) +{ + a[0] = _accel.x; + a[1] = _accel.y; + a[2] = _accel.z; +} + +void AP_InertialSensor_Oilpan::get_sensors( float * sensors ) +{ + sensors[0] = _gyro.x; + sensors[1] = _gyro.y; + sensors[2] = _gyro.z; + sensors[3] = _accel.x; + sensors[4] = _accel.y; + sensors[5] = _accel.z; +} + +float AP_InertialSensor_Oilpan::temperature() { return _temp; } + +uint32_t AP_InertialSensor_Oilpan::sample_time() { return _sample_time; } +void AP_InertialSensor_Oilpan::reset_sample_time() { } + +/* ------ Private functions -------------------------------------------*/ + +float AP_InertialSensor_Oilpan::_gyro_apply_std_offset( uint16_t adc_value ) +{ + /* Magic number from AP_ADC_Oilpan.h */ + return ((float) adc_value ) - 1658.0f; +} + +float AP_InertialSensor_Oilpan::_accel_apply_std_offset( uint16_t adc_value ) +{ + /* Magic number from AP_ADC_Oilpan.h */ + return ((float) adc_value ) - 2041.0f; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h new file mode 100644 index 0000000000..61874138d5 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -0,0 +1,60 @@ + +#ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ +#define __AP_INERTIAL_SENSOR_OILPAN_H__ + +#include +#include + +#include "../AP_ADC/AP_ADC.h" +#include "../AP_Math/AP_Math.h" +#include "AP_InertialSensor.h" + +class AP_InertialSensor_Oilpan : public AP_InertialSensor +{ + public: + + AP_InertialSensor_Oilpan( AP_ADC * adc ); + + /* Concrete implementation of AP_InertialSensor functions: */ + void init(AP_PeriodicProcess * scheduler); + bool update(); + float gx(); + float gy(); + float gz(); + void get_gyros( float * ); + float ax(); + float ay(); + float az(); + void get_accels( float * ); + void get_sensors( float * ); + float temperature(); + uint32_t sample_time(); + void reset_sample_time(); + + private: + + AP_ADC *_adc; + Vector3f _gyro; + Vector3f _accel; + float _temp; + + uint32_t _sample_time; + + static const uint8_t _sensors[6]; + static const int8_t _sensor_signs[6]; + static const uint8_t _gyro_temp_ch; + + static const float _gravity; + static const float _accel_scale; + + static const float _gyro_gain_x; + static const float _gyro_gain_y; + static const float _gyro_gain_z; + + static const float _adc_constraint; + + float _gyro_apply_std_offset( uint16_t adc_value ); + float _accel_apply_std_offset( uint16_t adc_value ); +}; + +#endif // __AP_INERTIAL_SENSOR_OILPAN_H__