mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: use delta_velocity/dt for calibration if available
This commit is contained in:
parent
423160eaf8
commit
b2b42e081a
|
@ -518,7 +518,13 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
|
||||||
update();
|
update();
|
||||||
// capture sample
|
// capture sample
|
||||||
for (uint8_t k=0; k<num_accels; k++) {
|
for (uint8_t k=0; k<num_accels; k++) {
|
||||||
samples[k][i] += get_accel(k);
|
Vector3f samp;
|
||||||
|
if(get_delta_velocity(k,samp)) {
|
||||||
|
samp /= _delta_velocity_dt[k];
|
||||||
|
} else {
|
||||||
|
samp = get_accel(k);
|
||||||
|
}
|
||||||
|
samples[k][i] += samp;
|
||||||
if (!get_accel_health(k)) {
|
if (!get_accel_health(k)) {
|
||||||
interact->printf_P(PSTR("accel[%u] not healthy"), (unsigned)k);
|
interact->printf_P(PSTR("accel[%u] not healthy"), (unsigned)k);
|
||||||
goto failed;
|
goto failed;
|
||||||
|
|
Loading…
Reference in New Issue