Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AHRS: added AHRS_WIND_MAX option

Browse Source 
this allows APM to cope better with airspeed sensor failure, but
ensuring airspeed stays within AHRS_WIND_MAX of ground speed
This commit is contained in:
Andrew Tridgell 2012-09-08 11:27:12 +10:00
parent c983de9d8b
commit 8f70a24fd4
3 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -41,6 +41,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Increment: .01
AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4),
// @Param: WIND_MAX
// @DisplayName: Maximum wind
// @Description: This sets the maximum allowable difference between ground speed and airspeed. This allows the plane to cope with a failing airspeed sensor. A value of zero means to use the airspeed as is.
// @Range: 0 127
// QUnits: m/s
// @Increment: 1
AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0),
AP_GROUPEND
};
@ -62,6 +70,13 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
{
if (_airspeed && _airspeed->use()) {
*airspeed_ret = _airspeed->get_airspeed();
if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
*airspeed_ret = constrain(*airspeed_ret,
_gps->ground_speed*0.01 - _wind_max,
_gps->ground_speed*0.01 + _wind_max);
}
return true;
}
return false;

1
libraries/AP_AHRS/AP_AHRS.h
View File

@ -143,6 +143,7 @@ public:
AP_Float _kp;
AP_Float gps_gain;
AP_Int8 _gps_use;
AP_Int8 _wind_max;
// for holding parameters
static const struct AP_Param::GroupInfo var_info[];

17
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -632,8 +632,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - airspeed;
_wind = _wind * 0.92 + wind * 0.08;
}
}
}
@ -699,16 +698,24 @@ bool AP_AHRS_DCM::get_position(struct Location *loc)
// return an airspeed estimate if available
bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
{
bool ret = false;
if (_airspeed && _airspeed->use()) {
*airspeed_ret = _airspeed->get_airspeed();
return true;
ret = true;
}
// estimate it via GPS speed and wind
if (have_gps()) {
*airspeed_ret = _last_airspeed;
return true;
ret = true;
}
return false;
if (ret && _wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
*airspeed_ret = constrain(*airspeed_ret,
_gps->ground_speed*0.01 - _wind_max,
_gps->ground_speed*0.01 + _wind_max);
}
return ret;
}