ardupilot/libraries/AP_IMU/AP_IMU_INS.cpp

242 lines
5.8 KiB
C++

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//
// AP_IMU_INS.cpp - IMU Sensor Library for Ardupilot Mega
// Code by Michael Smith, Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
//
// 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_INS.cpp
/// @brief IMU driver on top of an INS driver. Provides calibration for the
// inertial sensors (gyro and accel)
#include <FastSerial.h>
#include <AP_Common.h>
#include <avr/eeprom.h>
#include "AP_IMU_INS.h"
void
AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler )
{
_ins->init(scheduler);
// 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(delay_cb, flash_leds_cb);
// save calibration
_sensor_cal.save();
}
}
/**************************************************/
void
AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
_init_gyro(delay_cb, flash_leds_cb);
_sensor_cal.save();
}
#define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0)
void
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[3] = {0,0,0};
float total_change;
float max_offset;
float ins_gyro[6];
// cold start
delay_cb(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
FLASH_LEDS(true);
delay_cb(20);
_ins->update();
_ins->get_gyros(ins_gyro);
FLASH_LEDS(false);
delay_cb(20);
}
for (int j = 0; j <= 2; j++)
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
_ins->update();
_ins->get_gyros(ins_gyro);
for (int j = 0; j <= 2; j++){
prev[j] = _sensor_cal[j];
adc_in = ins_gyro[j];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){
_ins->update();
_ins->get_gyros(ins_gyro);
for (int j = 0; j < 3; j++){
adc_in = ins_gyro[j];
// filter
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
delay_cb(20);
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
FLASH_LEDS(false);
}
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_cb(500);
} while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
}
void
AP_IMU_INS::save()
{
_sensor_cal.save();
}
void
AP_IMU_INS::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
_init_accel(delay_cb, flash_leds_cb);
_sensor_cal.save();
}
void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0};
float total_change;
float max_offset;
float ins_accel[3];
// cold start
delay_cb(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 {
_ins->update();
_ins->get_accels(ins_accel);
for (int j = 3; j <= 5; j++){
prev[j] = _sensor_cal[j];
adc_in = ins_accel[j-3];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){ // We take some readings...
delay_cb(20);
_ins->update();
_ins->get_accels(ins_accel);
for (int j = 3; j < 6; j++){
adc_in = ins_accel[j-3];
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
FLASH_LEDS(false);
}
flashcount++;
}
// null gravity from the Z accel
_sensor_cal[5] += 9.805;
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_cb(500);
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
Serial.printf_P(PSTR(" "));
}
float
AP_IMU_INS::_calibrated(uint8_t channel, float ins_value)
{
return ins_value - _sensor_cal[channel];
}
bool
AP_IMU_INS::update(void)
{
float gyros[3];
float accels[3];
_ins->update();
_ins->get_gyros(gyros);
_ins->get_accels(accels);
_sample_time = _ins->sample_time();
// convert corrected gyro readings to delta acceleration
//
_gyro.x = _calibrated(0, gyros[0]);
_gyro.y = _calibrated(1, gyros[1]);
_gyro.z = _calibrated(2, gyros[2]);
// convert corrected accelerometer readings to acceleration
//
_accel.x = _calibrated(3, accels[0]);
_accel.y = _calibrated(4, accels[1]);
_accel.z = _calibrated(5, accels[2]);
_accel_filtered.x = _accel_filtered.x / 2 + _accel.x / 2;
_accel_filtered.y = _accel_filtered.y / 2 + _accel.y / 2;
_accel_filtered.z = _accel_filtered.z / 2 + _accel.z / 2;
// always updated
return true;
}