You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
219 lines
6.4 KiB
219 lines
6.4 KiB
/* |
|
APM_IMU.cpp - IMU Sensor Library for Ardupilot Mega |
|
Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com |
|
|
|
This library works with the ArduPilot Mega and "Oilpan" |
|
|
|
This library is free software; you can redistribute it and/or |
|
modify it under the terms of the GNU Lesser General Public |
|
License as published by the Free Software Foundation; either |
|
version 2.1 of the License, or (at your option) any later version. |
|
|
|
Methods: |
|
quick_init() : For air restart |
|
init() : Calibration |
|
gyro_init() : For ground start using saved accel offsets |
|
get_gyro() : Returns gyro vector. Elements in radians/second |
|
get_accel() : Returns acceleration vector. Elements in meters/seconds squared |
|
|
|
*/ |
|
|
|
#include <AP_IMU.h> |
|
|
|
#define A_LED_PIN 37 //37 = A, 35 = C |
|
#define C_LED_PIN 35 |
|
|
|
// 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 |
|
#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer |
|
#define accel_scale(x) (x*9.80665/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared |
|
|
|
#define ToRad(x) (x*0.01745329252) // *pi/180 |
|
#define ToDeg(x) (x*57.2957795131) // *180/pi |
|
|
|
// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 |
|
// Tested values : 0.4026, ?, 0.4192 |
|
#define _gyro_gain_x 0.4 //X axis Gyro gain |
|
#define _gyro_gain_y 0.41 //Y axis Gyro gain |
|
#define _gyro_gain_z 0.41 //Z axis Gyro |
|
|
|
#define ADC_CONSTRAINT 900 |
|
|
|
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ |
|
const uint8_t AP_IMU::_sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware |
|
const int AP_IMU::_sensor_signs[] = { 1, -1, -1, |
|
1, -1, -1}; |
|
|
|
// Temp compensation curve constants |
|
// These must be produced by measuring data and curve fitting |
|
// [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] |
|
const float AP_IMU::_gyro_temp_curve[3][3] = { |
|
{1665,0,0}, |
|
{1665,0,0}, |
|
{1665,0,0} |
|
}; // To Do - make additional constructors to pass this in. |
|
|
|
|
|
|
|
// Constructors //////////////////////////////////////////////////////////////// |
|
|
|
AP_IMU::AP_IMU(void) |
|
{ |
|
} |
|
|
|
|
|
/**************************************************/ |
|
void |
|
AP_IMU::quick_init(uint16_t *_offset_address) |
|
{ |
|
// We read the imu offsets from EEPROM for a quick air restart |
|
eeprom_read_block((void*)&_adc_offset, _offset_address, sizeof(_adc_offset)); |
|
|
|
} |
|
|
|
|
|
/**************************************************/ |
|
void |
|
AP_IMU::read_offsets(void) |
|
{ |
|
|
|
float temp; |
|
int flashcount = 0; |
|
int tc_temp = APM_ADC.Ch(_gyro_temp_ch); |
|
delay(500); |
|
|
|
for(int c = 0; c < 200; c++) |
|
{ |
|
digitalWrite(A_LED_PIN, LOW); |
|
digitalWrite(C_LED_PIN, HIGH); |
|
delay(20); |
|
for (int i = 0; i < 6; i++) |
|
_adc_in[i] = APM_ADC.Ch(_sensors[i]); |
|
digitalWrite(C_LED_PIN, LOW); |
|
digitalWrite(A_LED_PIN, HIGH); |
|
delay(20); |
|
} |
|
|
|
for(int i = 0; i < 200; i++){ // We take some readings... |
|
for (int j = 0; j < 6; j++) { |
|
_adc_in[j] = APM_ADC.Ch(_sensors[j]); |
|
if (j < 3) { |
|
_adc_in[j] -= gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias |
|
} else { |
|
_adc_in[j] -= 2025; |
|
} |
|
|
|
_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1; |
|
|
|
} |
|
|
|
delay(20); |
|
if(flashcount == 5) { |
|
digitalWrite(A_LED_PIN, LOW); |
|
digitalWrite(C_LED_PIN, HIGH); |
|
} |
|
if(flashcount >= 10) { |
|
flashcount = 0; |
|
digitalWrite(C_LED_PIN, LOW); |
|
digitalWrite(A_LED_PIN, HIGH); |
|
} |
|
flashcount++; |
|
} |
|
|
|
_adc_offset[5] += GRAVITY * _sensor_signs[5]; |
|
|
|
} |
|
|
|
/**************************************************/ |
|
void |
|
AP_IMU::init(uint16_t *_offset_address) |
|
{ |
|
|
|
read_offsets(); |
|
|
|
// Save offset values to EEPROM for use in an air restart, gyro only restart |
|
eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset)); |
|
|
|
} |
|
|
|
|
|
/**************************************************/ |
|
void |
|
AP_IMU::gyro_init(uint16_t *_offset_address) |
|
{ |
|
float temp[6]; |
|
read_offsets(); |
|
// We read the imu offsets from EEPROM to reuse the saved accel values. |
|
// This way we do not need the IMU to be level during calibration. |
|
eeprom_read_block((void*)&temp, _offset_address, sizeof(_adc_offset)); |
|
_adc_offset[3] = temp[3]; |
|
_adc_offset[4] = temp[4]; |
|
_adc_offset[5] = temp[5]; |
|
|
|
// Save offset values to EEPROM for use in an air restart |
|
eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset)); |
|
} |
|
|
|
|
|
/**************************************************/ |
|
// Returns the temperature compensated raw gyro value |
|
//--------------------------------------------------- |
|
float |
|
AP_IMU::gyro_temp_comp(int i, int temp) const |
|
{ |
|
// We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 |
|
//------------------------------------------------------------------------ |
|
return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp; |
|
} |
|
/**************************************************/ |
|
Vector3f |
|
AP_IMU::get_gyro(void) |
|
{ |
|
int tc_temp = APM_ADC.Ch(_gyro_temp_ch); |
|
|
|
for (int i = 0; i < 3; i++) { |
|
_adc_in[i] = APM_ADC.Ch(_sensors[i]); |
|
_adc_in[i] -= gyro_temp_comp(i,tc_temp); // Subtract temp compensated typical gyro bias |
|
if (_sensor_signs[i] < 0) |
|
_adc_in[i] = (_adc_offset[i] - _adc_in[i]); |
|
else |
|
_adc_in[i] = (_adc_in[i] - _adc_offset[i]); |
|
|
|
if (abs(_adc_in[i]) > ADC_CONSTRAINT) { |
|
adc_constraints++; // We keep track of the number of times |
|
_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values |
|
} |
|
} |
|
|
|
_gyro_vector.x = ToRad(_gyro_gain_x) * _adc_in[0]; |
|
_gyro_vector.y = ToRad(_gyro_gain_y) * _adc_in[1]; |
|
_gyro_vector.z = ToRad(_gyro_gain_z) * _adc_in[2]; |
|
|
|
return _gyro_vector; |
|
} |
|
|
|
/**************************************************/ |
|
Vector3f |
|
AP_IMU::get_accel(void) |
|
{ |
|
for (int i = 3; i < 6; i++) { |
|
_adc_in[i] = APM_ADC.Ch(_sensors[i]); |
|
_adc_in[i] -= 2025; // Subtract typical accel bias |
|
if (_sensor_signs[i] < 0) |
|
_adc_in[i] = (_adc_offset[i] - _adc_in[i]); |
|
else |
|
_adc_in[i] = (_adc_in[i] - _adc_offset[i]); |
|
if (abs(_adc_in[i]) > ADC_CONSTRAINT) { |
|
adc_constraints++; // We keep track of the number of times |
|
_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values |
|
} |
|
} |
|
|
|
_accel_vector.x = accel_scale(_adc_in[3]); |
|
_accel_vector.y = accel_scale(_adc_in[4]); |
|
_accel_vector.z = accel_scale(_adc_in[5]); |
|
|
|
return _accel_vector; |
|
} |
|
|
|
|