Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_IMU/AP_IMU_INS.cpp

This commit is contained in:
uncrustify 2012-08-16 23:19:49 -07:00 committed by Pat Hickey
parent af92d8a690
commit a704f06119
1 changed files with 74 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
libraries/AP_IMU/AP_IMU_INS.cpp
View File

@ -22,10 +22,10 @@
#include "AP_IMU_INS.h" #include "AP_IMU_INS.h"
void void
AP_IMU_INS::init( Start_style style, AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t), void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on), void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler ) AP_PeriodicProcess * scheduler )
{ {
_product_id = _ins->init(scheduler); _product_id = _ins->init(scheduler);
// if we are warm-starting, load the calibration data from EEPROM and go // if we are warm-starting, load the calibration data from EEPROM and go
@ -60,22 +60,22 @@ AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(
float ins_gyro[3]; float ins_gyro[3];
float best_diff = 0; float best_diff = 0;
// cold start // cold start
delay_cb(100); delay_cb(100);
Serial.printf_P(PSTR("Init Gyro")); Serial.printf_P(PSTR("Init Gyro"));
for(int c = 0; c < 25; c++) { for(int c = 0; c < 25; c++) {
// Mostly we are just flashing the LED's here // Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still // to tell the user to keep the IMU still
FLASH_LEDS(true); FLASH_LEDS(true);
delay_cb(20); delay_cb(20);
_ins->update(); _ins->update();
_ins->get_gyros(ins_gyro); _ins->get_gyros(ins_gyro);
FLASH_LEDS(false); FLASH_LEDS(false);
delay_cb(20); delay_cb(20);
} }
// the strategy is to average 200 points over 1 second, then do it // the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of // again and see if the 2nd average is within a small margin of
@ -85,7 +85,7 @@ AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(
// we try to get a good calibration estimate for up to 10 seconds // we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds // if the gyros are stable, we should get it in 2 seconds
for (int j = 0; j <= 10; j++) { for (int j = 0; j <= 10; j++) {
Vector3f gyro_sum, gyro_avg, gyro_diff; Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm; float diff_norm;
uint8_t i; uint8_t i;
@ -152,68 +152,68 @@ AP_IMU_INS::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(
void void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{ {
int flashcount = 0; int flashcount = 0;
float adc_in; float adc_in;
float prev[6] = {0,0,0}; float prev[6] = {0,0,0};
float total_change; float total_change;
float max_offset; float max_offset;
float ins_accel[3]; float ins_accel[3];
// cold start // cold start
delay_cb(500); delay_cb(500);
Serial.printf_P(PSTR("Init Accel")); 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 for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do { do {
_ins->update(); _ins->update();
_ins->get_accels(ins_accel); _ins->get_accels(ins_accel);
for (int j = 3; j <= 5; j++){ for (int j = 3; j <= 5; j++) {
prev[j] = _sensor_cal[j]; prev[j] = _sensor_cal[j];
adc_in = ins_accel[j-3]; adc_in = ins_accel[j-3];
_sensor_cal[j] = adc_in; _sensor_cal[j] = adc_in;
} }
for(int i = 0; i < 50; i++){ // We take some readings... for(int i = 0; i < 50; i++) { // We take some readings...
delay_cb(20); delay_cb(20);
_ins->update(); _ins->update();
_ins->get_accels(ins_accel); _ins->get_accels(ins_accel);
for (int j = 3; j < 6; j++){ for (int j = 3; j < 6; j++) {
adc_in = ins_accel[j-3]; adc_in = ins_accel[j-3];
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
} }
if(flashcount == 5) { if(flashcount == 5) {
Serial.printf_P(PSTR("*")); Serial.printf_P(PSTR("*"));
FLASH_LEDS(true); FLASH_LEDS(true);
} }
if(flashcount >= 10) { if(flashcount >= 10) {
flashcount = 0; flashcount = 0;
FLASH_LEDS(false); FLASH_LEDS(false);
} }
flashcount++; flashcount++;
} }
// null gravity from the Z accel // null gravity from the Z accel
_sensor_cal[5] += 9.805; _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]); 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 = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5]; max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
delay_cb(500); delay_cb(500);
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset); } while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
Serial.printf_P(PSTR(" ")); Serial.printf_P(PSTR(" "));
} }
float float
AP_IMU_INS::_calibrated(uint8_t channel, float ins_value) AP_IMU_INS::_calibrated(uint8_t channel, float ins_value)
{ {
return ins_value - _sensor_cal[channel]; return ins_value - _sensor_cal[channel];
@ -223,28 +223,28 @@ AP_IMU_INS::_calibrated(uint8_t channel, float ins_value)
bool bool
AP_IMU_INS::update(void) AP_IMU_INS::update(void)
{ {
float gyros[3]; float gyros[3];
float accels[3]; float accels[3];
_ins->update(); _ins->update();
_ins->get_gyros(gyros); _ins->get_gyros(gyros);
_ins->get_accels(accels); _ins->get_accels(accels);
_sample_time = _ins->sample_time(); _sample_time = _ins->sample_time();
// convert corrected gyro readings to delta acceleration // convert corrected gyro readings to delta acceleration
// //
_gyro.x = _calibrated(0, gyros[0]); _gyro.x = _calibrated(0, gyros[0]);
_gyro.y = _calibrated(1, gyros[1]); _gyro.y = _calibrated(1, gyros[1]);
_gyro.z = _calibrated(2, gyros[2]); _gyro.z = _calibrated(2, gyros[2]);
// convert corrected accelerometer readings to acceleration // convert corrected accelerometer readings to acceleration
// //
_accel.x = _calibrated(3, accels[0]); _accel.x = _calibrated(3, accels[0]);
_accel.y = _calibrated(4, accels[1]); _accel.y = _calibrated(4, accels[1]);
_accel.z = _calibrated(5, accels[2]); _accel.z = _calibrated(5, accels[2]);
// always updated // always updated
return true; return true;
} }
bool AP_IMU_INS::new_data_available(void) { bool AP_IMU_INS::new_data_available(void) {