aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Airspeed: pass in max airspeed via function call instead of using aparm

Browse Source
This commit is contained in:
Tom Pittenger 2016-08-07 20:28:24 -07:00
parent ad697243b3
commit ae46c38ff7
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -23,7 +23,7 @@ public:
// take current airspeed in m/s and ground speed vector and return // take current airspeed in m/s and ground speed vector and return
// new scaling factor // new scaling factor
float update(float airspeed, const Vector3f &vg); float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal);
private: private:
// state of kalman filter for airspeed ratio estimation // state of kalman filter for airspeed ratio estimation
@ -125,7 +125,7 @@ public:
} }
// update airspeed ratio calibration // update airspeed ratio calibration
void update_calibration(const Vector3f &vground); void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
// log data to MAVLink // log data to MAVLink
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground); void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);

10
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

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