bool AP_InertialSensor_PX4::update(void)
{
while (num_samples_available() == 0) {
hal.scheduler->delay(1);
}
uint32_t now = hal.scheduler->micros();
// the current mpu6000 PX4 driver does not buffer samples, so
// using the sample count times 5ms would produce a bad delta time
// if we missed one. For now we need to use the clock to get the
// delta time
_delta_time = (now - _last_update_usec) * 1.0e-6f;
_last_update_usec = now;
Vector3f accel_scale = _accel_scale.get();
_accel = _accel_sum / _accel_sum_count;
_accel.rotate(_board_orientation);
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro = _gyro_sum / _gyro_sum_count;
_gyro.rotate(_board_orientation);
_gyro -= _gyro_offset;
_accel_sum.zero();
_accel_sum_count = 0;
_gyro_sum.zero();
_gyro_sum_count = 0;
return true;
}
void AP_InertialSensor_ITG3200::wait_for_sample()
{
uint32_t tstart = hal.scheduler->micros();
while (num_samples_available() == 0) {
uint32_t now = hal.scheduler->micros();
uint32_t dt = now - tstart;
if (dt > 50000) {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_ITG3200::update waited 50ms for data from interrupt"));
}
}
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
Vector3f samples[6];
Vector3f new_offsets;
Vector3f new_scaling;
Vector3f orig_offset;
Vector3f orig_scale;
// backup original offsets and scaling
orig_offset = _accel_offset.get();
orig_scale = _accel_scale.get();
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// capture data from 6 positions
for (int8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place APM %S and press any key.\n"), msg);
// wait for user input
interact->blocking_read();
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
hal.scheduler->delay(10);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
bool success = _calibrate_accel(samples, new_offsets, new_scaling);
interact->printf_P(PSTR("Offsets: %.2f %.2f %.2f\n"),
new_offsets.x, new_offsets.y, new_offsets.z);
interact->printf_P(PSTR("Scaling: %.2f %.2f %.2f\n"),
new_scaling.x, new_scaling.y, new_scaling.z);
if (success) {
interact->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
// calculate the trims as well and pass back to caller
_calculate_trim(samples[0], trim_roll, trim_pitch);
return true;
}
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
AP_InertialSensor_UserInteract* interact)
{
Vector3f samples[6];
Vector3f new_offsets;
Vector3f new_scaling;
Vector3f orig_offset;
Vector3f orig_scale;
// backup original offsets and scaling
orig_offset = _accel_offset.get();
orig_scale = _accel_scale.get();
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// capture data from 6 positions
for (int8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on it's left side");
break;
case 2:
msg = PSTR("on it's right side");
break;
case 3:
msg = PSTR("nose down");
break;
case 4:
msg = PSTR("nose up");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on it's back");
break;
}
interact->printf_P(
PSTR("Place APM %S and press any key.\n"), msg);
// wait for user input
interact->blocking_read();
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
hal.scheduler->delay(10);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
interact->printf_P(PSTR("Calibration successful\n"));
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
return true;
}
interact->printf_P(
PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
bool AP_InertialSensor_PX4::new_data_available(void)
{
return num_samples_available() > 0;
}
// perform accelerometer calibration including providing user instructions and feedback
bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
{
Vector3f samples[6];
Vector3f new_offsets;
Vector3f new_scaling;
Vector3f orig_offset;
Vector3f orig_scale;
// backup original offsets and scaling
orig_offset = _accel_offset.get();
orig_scale = _accel_scale.get();
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = Vector3f(1,1,1);
// capture data from 6 positions
for (int8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on it's left side");
break;
case 2:
msg = PSTR("on it's right side");
break;
case 3:
msg = PSTR("nose down");
break;
case 4:
msg = PSTR("nose up");
break;
case 5:
msg = PSTR("on it's back");
break;
}
if (send_msg == NULL) {
Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
}else{
send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
}
// wait for user input
while( !Serial.available() ) {
delay_cb(20);
}
// clear unput buffer
while( Serial.available() ) {
Serial.read();
}
// clear out any existing samples from ins
update();
// wait until we have 32 samples
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
delay_cb(1);
}
// read samples from ins
update();
// capture sample
samples[i] = get_accel();
}
// run the calibration routine
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
if (send_msg == NULL) {
Serial.printf_P(PSTR("Calibration successful\n"));
}else{
send_msg(PSTR("Calibration successful\n"));
}
// set and save calibration
_accel_offset.set(new_offsets);
_accel_scale.set(new_scaling);
_save_parameters();
return true;
}
if (send_msg == NULL) {
Serial.printf_P(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
} else {
send_msg(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
new_offsets.x, new_offsets.y, new_offsets.z,
new_scaling.x, new_scaling.y, new_scaling.z);
}
// restore original scaling and offsets
_accel_offset.set(orig_offset);
_accel_scale.set(orig_scale);
return false;
}
