INS: integrate AP_Notify
This commit is contained in:
parent
0797489ad8
commit
5651bdbe3a
@ -148,20 +148,20 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
|
||||
hal.scheduler->delay(100);
|
||||
hal.console->printf_P(PSTR("Init Gyro"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// remove existing gyro offsets
|
||||
_gyro_offset = Vector3f(0,0,0);
|
||||
|
||||
for(int8_t 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);
|
||||
hal.scheduler->delay(20);
|
||||
|
||||
update();
|
||||
ins_gyro = get_gyro();
|
||||
|
||||
FLASH_LEDS(false);
|
||||
hal.scheduler->delay(20);
|
||||
// give leds a chance to update
|
||||
AP_Notify::update();
|
||||
}
|
||||
|
||||
// the strategy is to average 200 points over 1 second, then do it
|
||||
@ -184,12 +184,9 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
|
||||
update();
|
||||
ins_gyro = get_gyro();
|
||||
gyro_sum += ins_gyro;
|
||||
if (i % 40 == 20) {
|
||||
FLASH_LEDS(true);
|
||||
} else if (i % 40 == 0) {
|
||||
FLASH_LEDS(false);
|
||||
}
|
||||
hal.scheduler->delay(5);
|
||||
// give leds a chance to update
|
||||
AP_Notify::update();
|
||||
}
|
||||
gyro_avg = gyro_sum / i;
|
||||
|
||||
@ -203,7 +200,8 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
|
||||
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
|
||||
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
|
||||
_gyro_offset = last_average;
|
||||
|
||||
// stop flashing leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
// all done
|
||||
return;
|
||||
} else if (diff_norm < best_diff) {
|
||||
@ -213,6 +211,9 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
|
||||
last_average = gyro_avg;
|
||||
}
|
||||
|
||||
// stop flashing leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
// we've kept the user waiting long enough - use the best pair we
|
||||
// found so far
|
||||
hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
|
||||
@ -245,6 +246,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
|
||||
|
||||
hal.console->printf_P(PSTR("Init Accel"));
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
||||
// clear accelerometer offsets and scaling
|
||||
_accel_offset = Vector3f(0,0,0);
|
||||
_accel_scale = Vector3f(1,1,1);
|
||||
@ -276,16 +280,14 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
|
||||
accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
|
||||
|
||||
// display some output to the user
|
||||
if(flashcount == 5) {
|
||||
hal.console->printf_P(PSTR("*"));
|
||||
FLASH_LEDS(true);
|
||||
}
|
||||
|
||||
if(flashcount >= 10) {
|
||||
hal.console->printf_P(PSTR("*"));
|
||||
flashcount = 0;
|
||||
FLASH_LEDS(false);
|
||||
}
|
||||
flashcount++;
|
||||
|
||||
// give leds a chance to update
|
||||
AP_Notify::update();
|
||||
}
|
||||
|
||||
// null gravity from the Z accel
|
||||
@ -301,6 +303,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
|
||||
// set the global accel offsets
|
||||
_accel_offset = accel_offset;
|
||||
|
||||
// stop flashing the leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
||||
hal.console->printf_P(PSTR(" "));
|
||||
|
||||
}
|
||||
|
@ -10,6 +10,7 @@
|
||||
#include <stdint.h>
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Notify.h>
|
||||
#include "AP_InertialSensor_UserInteract.h"
|
||||
/* AP_InertialSensor is an abstraction for gyro and accel measurements
|
||||
* which are correctly aligned to the body axes and scaled to SI units.
|
||||
|
Loading…
Reference in New Issue
Block a user