mirror of https://github.com/ArduPilot/ardupilot
281 lines
7.4 KiB
C++
281 lines
7.4 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
//
|
|
//
|
|
// AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega
|
|
// Code by Michael Smith, 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.
|
|
//
|
|
|
|
/// @file AP_IMU.h
|
|
/// @brief IMU driver for the APM oilpan
|
|
|
|
#include <FastSerial.h>
|
|
#include <AP_Common.h>
|
|
|
|
#include <avr/eeprom.h>
|
|
|
|
#include "AP_IMU_Oilpan.h"
|
|
|
|
// XXX secret knowledge about the APM/oilpan wiring
|
|
//
|
|
#define A_LED_PIN 37
|
|
#define C_LED_PIN 35
|
|
|
|
// Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
|
const uint8_t AP_IMU_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6}; // Channel assignments on the APM oilpan
|
|
const int8_t AP_IMU_Oilpan::_sensor_signs[6] = { 1,-1,-1, 1,-1,-1}; // Channel orientation vs. normal
|
|
|
|
// 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_Oilpan::_gyro_temp_curve[3][3] = {
|
|
{1658,0,0}, // Values to use if no temp compensation data available
|
|
{1658,0,0}, // Based on average values for 20 sample boards
|
|
{1658,0,0}
|
|
};
|
|
|
|
void
|
|
AP_IMU_Oilpan::init(Start_style style)
|
|
{
|
|
// if we are warm-starting, load the calibration data from EEPROM and go
|
|
//
|
|
if (WARM_START == style) {
|
|
_sensor_cal.load();
|
|
} else {
|
|
|
|
// do cold-start calibration for both accel and gyro
|
|
_init_gyro();
|
|
_init_accel();
|
|
|
|
// save calibration
|
|
_sensor_cal.save();
|
|
}
|
|
}
|
|
|
|
/**************************************************/
|
|
|
|
void
|
|
AP_IMU_Oilpan::init_gyro()
|
|
{
|
|
_init_gyro();
|
|
_sensor_cal.save();
|
|
}
|
|
|
|
void
|
|
AP_IMU_Oilpan::_init_gyro()
|
|
{
|
|
int flashcount = 0;
|
|
int tc_temp;
|
|
float adc_in;
|
|
float prev[3] = {0,0,0};
|
|
float total_change;
|
|
float max_offset;
|
|
|
|
// cold start
|
|
tc_temp = _adc->Ch(_gyro_temp_ch);
|
|
delay(500);
|
|
Serial.printf_P(PSTR("Init Gyro"));
|
|
|
|
for(int c = 0; c < 25; c++){ // Mostly we are just flashing the LED's here to tell the user to keep the IMU still
|
|
digitalWrite(A_LED_PIN, LOW);
|
|
digitalWrite(C_LED_PIN, HIGH);
|
|
delay(20);
|
|
|
|
for (int i = 0; i < 6; i++)
|
|
adc_in = _adc->Ch(_sensors[i]);
|
|
|
|
digitalWrite(A_LED_PIN, HIGH);
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
delay(20);
|
|
}
|
|
|
|
for (int j = 0; j <= 2; j++)
|
|
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
|
|
|
|
do {
|
|
for (int j = 0; j <= 2; j++){
|
|
prev[j] = _sensor_cal[j];
|
|
adc_in = _adc->Ch(_sensors[j]);
|
|
adc_in -= _sensor_compensation(j, tc_temp);
|
|
_sensor_cal[j] = adc_in;
|
|
}
|
|
|
|
for(int i = 0; i < 50; i++){
|
|
for (int j = 0; j < 3; j++){
|
|
adc_in = _adc->Ch(_sensors[j]);
|
|
// Subtract temp compensated typical gyro bias
|
|
adc_in -= _sensor_compensation(j, tc_temp);
|
|
// filter
|
|
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
|
|
}
|
|
|
|
delay(20);
|
|
if(flashcount == 5) {
|
|
Serial.printf_P(PSTR("*"));
|
|
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++;
|
|
}
|
|
|
|
total_change = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]);
|
|
max_offset = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
|
|
max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
|
|
delay(500);
|
|
} while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
|
|
}
|
|
|
|
void
|
|
AP_IMU_Oilpan::save()
|
|
{
|
|
_sensor_cal.save();
|
|
}
|
|
|
|
void
|
|
AP_IMU_Oilpan::init_accel()
|
|
{
|
|
_init_accel();
|
|
_sensor_cal.save();
|
|
}
|
|
|
|
void
|
|
AP_IMU_Oilpan::_init_accel()
|
|
{
|
|
int flashcount = 0;
|
|
float adc_in;
|
|
float prev[6] = {0,0,0};
|
|
float total_change;
|
|
float max_offset;
|
|
|
|
// cold start
|
|
delay(500);
|
|
|
|
Serial.printf_P(PSTR("Init Accel"));
|
|
|
|
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
|
|
|
|
do {
|
|
for (int j = 3; j <= 5; j++){
|
|
prev[j] = _sensor_cal[j];
|
|
adc_in = _adc->Ch(_sensors[j]);
|
|
adc_in -= _sensor_compensation(j, 0); // temperature ignored
|
|
_sensor_cal[j] = adc_in;
|
|
}
|
|
|
|
for(int i = 0; i < 50; i++){ // We take some readings...
|
|
|
|
delay(20);
|
|
|
|
for (int j = 3; j < 6; j++){
|
|
adc_in = _adc->Ch(_sensors[j]);
|
|
adc_in -= _sensor_compensation(j, 0); // temperature ignored
|
|
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
|
|
}
|
|
|
|
if(flashcount == 5) {
|
|
Serial.printf_P(PSTR("*"));
|
|
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++;
|
|
}
|
|
|
|
// null gravity from the Z accel
|
|
_sensor_cal[5] += _gravity * _sensor_signs[5];
|
|
|
|
total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]);
|
|
max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
|
|
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
|
|
|
|
delay(500);
|
|
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
|
|
|
|
Serial.printf_P(PSTR(" "));
|
|
}
|
|
|
|
/**************************************************/
|
|
// Returns the temperature compensated raw gyro value
|
|
//---------------------------------------------------
|
|
|
|
float
|
|
AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const
|
|
{
|
|
// do gyro temperature compensation
|
|
if (channel < 3) {
|
|
|
|
return _gyro_temp_curve[channel][0] +
|
|
_gyro_temp_curve[channel][1] * temperature +
|
|
_gyro_temp_curve[channel][2] * temperature * temperature;
|
|
}
|
|
|
|
// do fixed-offset accelerometer compensation
|
|
return 2041; // Average raw value from a 20 board sample
|
|
}
|
|
|
|
float
|
|
AP_IMU_Oilpan::_sensor_in(uint8_t channel, int temperature)
|
|
{
|
|
float adc_in;
|
|
|
|
// get the compensated sensor value
|
|
//
|
|
adc_in = _adc->Ch(_sensors[channel]) - _sensor_compensation(channel, temperature);
|
|
|
|
// adjust for sensor sign and apply calibration offset
|
|
//
|
|
if (_sensor_signs[channel] < 0) {
|
|
adc_in = _sensor_cal[channel] - adc_in;
|
|
} else {
|
|
adc_in = adc_in - _sensor_cal[channel];
|
|
}
|
|
|
|
// constrain sensor readings to the sensible range
|
|
//
|
|
if (fabs(adc_in) > _adc_constraint) {
|
|
adc_constraints++; // We keep track of the number of times
|
|
adc_in = constrain(adc_in, -_adc_constraint, _adc_constraint); // Throw out nonsensical values
|
|
}
|
|
return adc_in;
|
|
}
|
|
|
|
|
|
bool
|
|
AP_IMU_Oilpan::update(void)
|
|
{
|
|
int tc_temp = _adc->Ch(_gyro_temp_ch);
|
|
|
|
// convert corrected gyro readings to delta acceleration
|
|
//
|
|
_gyro.x = ToRad(_gyro_gain_x) * _sensor_in(0, tc_temp);
|
|
_gyro.y = ToRad(_gyro_gain_y) * _sensor_in(1, tc_temp);
|
|
_gyro.z = ToRad(_gyro_gain_z) * _sensor_in(2, tc_temp);
|
|
|
|
// convert corrected accelerometer readings to acceleration
|
|
//
|
|
_accel.x = _accel_scale * _sensor_in(3, tc_temp);
|
|
_accel.y = _accel_scale * _sensor_in(4, tc_temp);
|
|
_accel.z = _accel_scale * _sensor_in(5, tc_temp);
|
|
|
|
// always updated
|
|
return true;
|
|
}
|