|
|
|
@ -39,7 +39,7 @@ void Airspeed_Calibration::init(float initial_ratio)
@@ -39,7 +39,7 @@ void Airspeed_Calibration::init(float initial_ratio)
|
|
|
|
|
update the state of the airspeed calibration - needs to be called |
|
|
|
|
once a second |
|
|
|
|
*/ |
|
|
|
|
float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) |
|
|
|
|
float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal) |
|
|
|
|
{ |
|
|
|
|
// Perform the covariance prediction
|
|
|
|
|
// Q is a diagonal matrix so only need to add three terms in
|
|
|
|
@ -101,8 +101,8 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
@@ -101,8 +101,8 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
|
|
|
|
|
P.b.y = MAX(P.b.y, 0.0f); |
|
|
|
|
P.c.z = MAX(P.c.z, 0.0f); |
|
|
|
|
|
|
|
|
|
state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max); |
|
|
|
|
state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max); |
|
|
|
|
state.x = constrain_float(state.x, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal); |
|
|
|
|
state.y = constrain_float(state.y, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal); |
|
|
|
|
state.z = constrain_float(state.z, 0.5f, 1.0f); |
|
|
|
|
|
|
|
|
|
return state.z; |
|
|
|
@ -112,7 +112,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
@@ -112,7 +112,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
|
|
|
|
|
/*
|
|
|
|
|
called once a second to do calibration update |
|
|
|
|
*/ |
|
|
|
|
void AP_Airspeed::update_calibration(const Vector3f &vground) |
|
|
|
|
void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) |
|
|
|
|
{ |
|
|
|
|
if (!_autocal) { |
|
|
|
|
// auto-calibration not enabled
|
|
|
|
@ -130,7 +130,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground)
@@ -130,7 +130,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground)
|
|
|
|
|
float dpress = get_differential_pressure(); |
|
|
|
|
float true_airspeed = sqrtf(dpress) * _EAS2TAS; |
|
|
|
|
|
|
|
|
|
float zratio = _calibration.update(true_airspeed, vground); |
|
|
|
|
float zratio = _calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal); |
|
|
|
|
|
|
|
|
|
if (isnan(zratio) || isinf(zratio)) { |
|
|
|
|
return; |
|
|
|
|