/*
called once a second to do calibration update
*/
void AP_Airspeed::update_calibration(Vector3f vground)
{
if (!_autocal) {
// auto-calibration not enabled
return;
}
// calculate true airspeed, assuming a airspeed ratio of 1.0
float true_airspeed = sqrtf(get_differential_pressure()) * _EAS2TAS;
float ratio = _calibration.update(true_airspeed, vground);
if (isnan(ratio) || isinf(ratio)) {
return;
}
// this constrains the resulting ratio to between 1.5 and 3
ratio = constrain_float(ratio, 0.577f, 0.816f);
_ratio.set(1/sq(ratio));
if (_counter > 60) {
if (_last_saved_ratio < 1.05f*_ratio ||
_last_saved_ratio < 0.95f*_ratio) {
_ratio.save();
_last_saved_ratio = _ratio;
_counter = 0;
}
} else {
_counter++;
}
}
// log airspeed calibration data to MAVLink
void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
{
mavlink_msg_airspeed_autocal_send(chan,
vground.x,
vground.y,
vground.z,
get_differential_pressure(),
_EAS2TAS,
_ratio.get(),
_calibration.state.x,
_calibration.state.y,
_calibration.state.z,
_calibration.P.a.x,
_calibration.P.b.y,
_calibration.P.c.z);
}
/*
called once a second to do calibration update
*/
void AP_Airspeed::update_calibration(const Vector3f &vground)
{
if (!_autocal) {
// auto-calibration not enabled
return;
}
// set state.z based on current ratio, this allows the operator to
// override the current ratio in flight with autocal, which is
// very useful both for testing and to force a reasonable value.
float ratio = constrain_float(_ratio, 1.0f, 4.0f);
_calibration.state.z = 1.0 / sqrtf(ratio);
// calculate true airspeed, assuming a airspeed ratio of 1.0
float dpress = get_differential_pressure();
float true_airspeed = sqrtf(dpress) * _EAS2TAS;
float zratio = _calibration.update(true_airspeed, vground);
if (isnan(zratio) || isinf(zratio)) {
return;
}
// this constrains the resulting ratio to between 1.0 and 4.0
zratio = constrain_float(zratio, 0.5f, 1.0f);
_ratio.set(1/sq(zratio));
if (_counter > 60) {
if (_last_saved_ratio > 1.05f*_ratio ||
_last_saved_ratio < 0.95f*_ratio) {
_ratio.save();
_last_saved_ratio = _ratio;
_counter = 0;
}
} else {
_counter++;
}
}
