mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 05:58:30 -04:00
InertialSensor: reorder .cpp file to match .h
No functional changes
This commit is contained in:
Randy Mackay
parent
818b3b74f6
commit
50ae5b2519
@ -131,126 +131,6 @@ AP_InertialSensor::init( Start_style style,
|
||||
}
|
||||
}
|
||||
|
||||
// save parameters to eeprom
|
||||
void AP_InertialSensor::_save_parameters()
|
||||
{
|
||||
_product_id.save();
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
_accel_scale[i].save();
|
||||
_accel_offset[i].save();
|
||||
_gyro_offset[i].save();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::init_gyro()
|
||||
{
|
||||
_init_gyro();
|
||||
|
||||
// save calibration
|
||||
_save_parameters();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_gyro()
|
||||
{
|
||||
uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
|
||||
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
|
||||
float best_diff[INS_MAX_INSTANCES];
|
||||
bool converged[INS_MAX_INSTANCES];
|
||||
|
||||
// cold start
|
||||
hal.console->print_P(PSTR("Init Gyro"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// remove existing gyro offsets
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
_gyro_offset[k] = Vector3f(0,0,0);
|
||||
best_diff[k] = 0;
|
||||
last_average[k].zero();
|
||||
converged[k] = false;
|
||||
}
|
||||
|
||||
for(int8_t c = 0; c < 5; c++) {
|
||||
hal.scheduler->delay(5);
|
||||
update();
|
||||
}
|
||||
|
||||
// the strategy is to average 50 points over 0.5 seconds, then do it
|
||||
// again and see if the 2nd average is within a small margin of
|
||||
// the first
|
||||
|
||||
uint8_t num_converged = 0;
|
||||
|
||||
// we try to get a good calibration estimate for up to 10 seconds
|
||||
// if the gyros are stable, we should get it in 1 second
|
||||
for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) {
|
||||
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
|
||||
float diff_norm[INS_MAX_INSTANCES];
|
||||
uint8_t i;
|
||||
|
||||
memset(diff_norm, 0, sizeof(diff_norm));
|
||||
|
||||
hal.console->print_P(PSTR("*"));
|
||||
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_sum[k].zero();
|
||||
}
|
||||
for (i=0; i<50; i++) {
|
||||
update();
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_sum[k] += get_gyro(k);
|
||||
}
|
||||
hal.scheduler->delay(5);
|
||||
}
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_avg[k] = gyro_sum[k] / i;
|
||||
gyro_diff[k] = last_average[k] - gyro_avg[k];
|
||||
diff_norm[k] = gyro_diff[k].length();
|
||||
}
|
||||
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
if (converged[k]) continue;
|
||||
if (j == 0) {
|
||||
best_diff[k] = diff_norm[k];
|
||||
best_avg[k] = gyro_avg[k];
|
||||
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
|
||||
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
|
||||
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
|
||||
_gyro_offset[k] = last_average[k];
|
||||
converged[k] = true;
|
||||
num_converged++;
|
||||
} else if (diff_norm[k] < best_diff[k]) {
|
||||
best_diff[k] = diff_norm[k];
|
||||
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
|
||||
}
|
||||
last_average[k] = gyro_avg[k];
|
||||
}
|
||||
}
|
||||
|
||||
// stop flashing leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
if (num_converged == num_gyros) {
|
||||
// all OK
|
||||
return;
|
||||
}
|
||||
|
||||
// we've kept the user waiting long enough - use the best pair we
|
||||
// found so far
|
||||
hal.console->println();
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
if (!converged[k]) {
|
||||
hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
|
||||
(unsigned)k, ToDeg(best_diff[k]));
|
||||
_gyro_offset[k] = best_avg[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
AP_InertialSensor::init_accel()
|
||||
{
|
||||
@ -260,106 +140,6 @@ AP_InertialSensor::init_accel()
|
||||
_save_parameters();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_accel()
|
||||
{
|
||||
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
|
||||
uint8_t flashcount = 0;
|
||||
Vector3f prev[INS_MAX_INSTANCES];
|
||||
Vector3f accel_offset[INS_MAX_INSTANCES];
|
||||
float total_change[INS_MAX_INSTANCES];
|
||||
float max_offset[INS_MAX_INSTANCES];
|
||||
|
||||
memset(max_offset, 0, sizeof(max_offset));
|
||||
memset(total_change, 0, sizeof(total_change));
|
||||
|
||||
// cold start
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
hal.console->print_P(PSTR("Init Accel"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// clear accelerometer offsets and scaling
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k] = Vector3f(0,0,0);
|
||||
_accel_scale[k] = Vector3f(1,1,1);
|
||||
|
||||
// initialise accel offsets to a large value the first time
|
||||
// this will force us to calibrate accels at least twice
|
||||
accel_offset[k] = Vector3f(500, 500, 500);
|
||||
}
|
||||
|
||||
// loop until we calculate acceptable offsets
|
||||
while (true) {
|
||||
// get latest accelerometer values
|
||||
update();
|
||||
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
// store old offsets
|
||||
prev[k] = accel_offset[k];
|
||||
|
||||
// get new offsets
|
||||
accel_offset[k] = get_accel(k);
|
||||
}
|
||||
|
||||
// We take some readings...
|
||||
for(int8_t i = 0; i < 50; i++) {
|
||||
|
||||
hal.scheduler->delay(20);
|
||||
update();
|
||||
|
||||
// low pass filter the offsets
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
|
||||
}
|
||||
|
||||
// display some output to the user
|
||||
if(flashcount >= 10) {
|
||||
hal.console->print_P(PSTR("*"));
|
||||
flashcount = 0;
|
||||
}
|
||||
flashcount++;
|
||||
}
|
||||
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
// null gravity from the Z accel
|
||||
accel_offset[k].z += GRAVITY_MSS;
|
||||
|
||||
total_change[k] =
|
||||
fabsf(prev[k].x - accel_offset[k].x) +
|
||||
fabsf(prev[k].y - accel_offset[k].y) +
|
||||
fabsf(prev[k].z - accel_offset[k].z);
|
||||
max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
|
||||
max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
|
||||
}
|
||||
|
||||
uint8_t num_converged = 0;
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE &&
|
||||
max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
|
||||
num_converged++;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_converged == num_accels) break;
|
||||
|
||||
hal.scheduler->delay(500);
|
||||
}
|
||||
|
||||
// set the global accel offsets
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k] = accel_offset[k];
|
||||
}
|
||||
|
||||
// stop flashing the leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
hal.console->print_P(PSTR(" "));
|
||||
|
||||
}
|
||||
|
||||
#if !defined( __AVR_ATmega1280__ )
|
||||
// calibrate_accel - perform accelerometer calibration including providing user
|
||||
// instructions and feedback Gauss-Newton accel calibration routines borrowed
|
||||
@ -489,6 +269,7 @@ failed:
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
/// calibrated - returns true if the accelerometers have been calibrated
|
||||
/// @note this should not be called while flying because it reads from the eeprom which can be slow
|
||||
@ -504,6 +285,215 @@ bool AP_InertialSensor::calibrated()
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::init_gyro()
|
||||
{
|
||||
_init_gyro();
|
||||
|
||||
// save calibration
|
||||
_save_parameters();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_accel()
|
||||
{
|
||||
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
|
||||
uint8_t flashcount = 0;
|
||||
Vector3f prev[INS_MAX_INSTANCES];
|
||||
Vector3f accel_offset[INS_MAX_INSTANCES];
|
||||
float total_change[INS_MAX_INSTANCES];
|
||||
float max_offset[INS_MAX_INSTANCES];
|
||||
|
||||
memset(max_offset, 0, sizeof(max_offset));
|
||||
memset(total_change, 0, sizeof(total_change));
|
||||
|
||||
// cold start
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
hal.console->print_P(PSTR("Init Accel"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// clear accelerometer offsets and scaling
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k] = Vector3f(0,0,0);
|
||||
_accel_scale[k] = Vector3f(1,1,1);
|
||||
|
||||
// initialise accel offsets to a large value the first time
|
||||
// this will force us to calibrate accels at least twice
|
||||
accel_offset[k] = Vector3f(500, 500, 500);
|
||||
}
|
||||
|
||||
// loop until we calculate acceptable offsets
|
||||
while (true) {
|
||||
// get latest accelerometer values
|
||||
update();
|
||||
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
// store old offsets
|
||||
prev[k] = accel_offset[k];
|
||||
|
||||
// get new offsets
|
||||
accel_offset[k] = get_accel(k);
|
||||
}
|
||||
|
||||
// We take some readings...
|
||||
for(int8_t i = 0; i < 50; i++) {
|
||||
|
||||
hal.scheduler->delay(20);
|
||||
update();
|
||||
|
||||
// low pass filter the offsets
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
|
||||
}
|
||||
|
||||
// display some output to the user
|
||||
if(flashcount >= 10) {
|
||||
hal.console->print_P(PSTR("*"));
|
||||
flashcount = 0;
|
||||
}
|
||||
flashcount++;
|
||||
}
|
||||
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
// null gravity from the Z accel
|
||||
accel_offset[k].z += GRAVITY_MSS;
|
||||
|
||||
total_change[k] =
|
||||
fabsf(prev[k].x - accel_offset[k].x) +
|
||||
fabsf(prev[k].y - accel_offset[k].y) +
|
||||
fabsf(prev[k].z - accel_offset[k].z);
|
||||
max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
|
||||
max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
|
||||
}
|
||||
|
||||
uint8_t num_converged = 0;
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE &&
|
||||
max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
|
||||
num_converged++;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_converged == num_accels) break;
|
||||
|
||||
hal.scheduler->delay(500);
|
||||
}
|
||||
|
||||
// set the global accel offsets
|
||||
for (uint8_t k=0; k<num_accels; k++) {
|
||||
_accel_offset[k] = accel_offset[k];
|
||||
}
|
||||
|
||||
// stop flashing the leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
hal.console->print_P(PSTR(" "));
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_gyro()
|
||||
{
|
||||
uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
|
||||
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
|
||||
float best_diff[INS_MAX_INSTANCES];
|
||||
bool converged[INS_MAX_INSTANCES];
|
||||
|
||||
// cold start
|
||||
hal.console->print_P(PSTR("Init Gyro"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// remove existing gyro offsets
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
_gyro_offset[k] = Vector3f(0,0,0);
|
||||
best_diff[k] = 0;
|
||||
last_average[k].zero();
|
||||
converged[k] = false;
|
||||
}
|
||||
|
||||
for(int8_t c = 0; c < 5; c++) {
|
||||
hal.scheduler->delay(5);
|
||||
update();
|
||||
}
|
||||
|
||||
// the strategy is to average 50 points over 0.5 seconds, then do it
|
||||
// again and see if the 2nd average is within a small margin of
|
||||
// the first
|
||||
|
||||
uint8_t num_converged = 0;
|
||||
|
||||
// we try to get a good calibration estimate for up to 10 seconds
|
||||
// if the gyros are stable, we should get it in 1 second
|
||||
for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) {
|
||||
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
|
||||
float diff_norm[INS_MAX_INSTANCES];
|
||||
uint8_t i;
|
||||
|
||||
memset(diff_norm, 0, sizeof(diff_norm));
|
||||
|
||||
hal.console->print_P(PSTR("*"));
|
||||
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_sum[k].zero();
|
||||
}
|
||||
for (i=0; i<50; i++) {
|
||||
update();
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_sum[k] += get_gyro(k);
|
||||
}
|
||||
hal.scheduler->delay(5);
|
||||
}
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
gyro_avg[k] = gyro_sum[k] / i;
|
||||
gyro_diff[k] = last_average[k] - gyro_avg[k];
|
||||
diff_norm[k] = gyro_diff[k].length();
|
||||
}
|
||||
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
if (converged[k]) continue;
|
||||
if (j == 0) {
|
||||
best_diff[k] = diff_norm[k];
|
||||
best_avg[k] = gyro_avg[k];
|
||||
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
|
||||
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
|
||||
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
|
||||
_gyro_offset[k] = last_average[k];
|
||||
converged[k] = true;
|
||||
num_converged++;
|
||||
} else if (diff_norm[k] < best_diff[k]) {
|
||||
best_diff[k] = diff_norm[k];
|
||||
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
|
||||
}
|
||||
last_average[k] = gyro_avg[k];
|
||||
}
|
||||
}
|
||||
|
||||
// stop flashing leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
if (num_converged == num_gyros) {
|
||||
// all OK
|
||||
return;
|
||||
}
|
||||
|
||||
// we've kept the user waiting long enough - use the best pair we
|
||||
// found so far
|
||||
hal.console->println();
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
if (!converged[k]) {
|
||||
hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
|
||||
(unsigned)k, ToDeg(best_diff[k]));
|
||||
_gyro_offset[k] = best_avg[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if !defined( __AVR_ATmega1280__ )
|
||||
// _calibrate_model - perform low level accel calibration
|
||||
// accel_sample are accelerometer samples collected in 6 different positions
|
||||
// accel_offsets are output from the calibration routine
|
||||
@ -679,3 +669,13 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll,
|
||||
|
||||
#endif // __AVR_ATmega1280__
|
||||
|
||||
// save parameters to eeprom
|
||||
void AP_InertialSensor::_save_parameters()
|
||||
{
|
||||
_product_id.save();
|
||||
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
|
||||
_accel_scale[i].save();
|
||||
_accel_offset[i].save();
|
||||
_gyro_offset[i].save();
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user