mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
purple: added AP_InertialSensor library
this abstracts the way of getting inertial sensor (gyro and accelerometer) data for the APM1 and purple hardware. The Oilpan code is based closely on the old APM1 code
This commit is contained in:
Pat Hickey
parent
c9f7618ccc
commit
82b48784ef
61
libraries/AP_InertialSensor/AP_InertialSensor.h
Normal file
61
libraries/AP_InertialSensor/AP_InertialSensor.h
Normal file
@ -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__
|
||||
298
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
Normal file
298
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
Normal file
@ -0,0 +1,298 @@
|
||||
#include <FastSerial.h>
|
||||
|
||||
#include "AP_InertialSensor_MPU6000.h"
|
||||
|
||||
#include <wiring.h>
|
||||
#include <SPI.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
67
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
Normal file
67
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
Normal file
@ -0,0 +1,67 @@
|
||||
|
||||
#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_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__
|
||||
122
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
Normal file
122
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
Normal file
@ -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;
|
||||
}
|
||||
60
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
Normal file
60
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
Normal file
@ -0,0 +1,60 @@
|
||||
|
||||
#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_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__
|
||||
Loading…
Reference in New Issue
Block a user