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

AP_AHRS: added true airspeed support in AHRS

Browse Source 
use true airspeed for wind calculations, and allow other drivers to
ask for the current ratio
This commit is contained in:
Andrew Tridgell 2013-07-20 11:11:57 +10:00
parent 3c66cb8af1
commit ec73fadc45
3 changed files with 34 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -109,9 +109,11 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
float gnd_speed = _gps->ground_speed_cm*0.01f;
*airspeed_ret = constrain_float(*airspeed_ret,
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,
gnd_speed + _wind_max);
*airspeed_ret = true_airspeed / get_EAS2TAS();
}
return true;
}
@ -145,27 +147,6 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
}
}
// correct a bearing in centi-degrees for wind
void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd)
{
if (!use_compass() || !_flags.wind_estimation) {
// we are not using the compass - no wind correction,
// as GPS gives course over ground already
return;
}
// if we are using a compass for navigation, then adjust the
// heading to account for wind
Vector3f wind = wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
float speed;
if (airspeed_estimate(&speed)) {
Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
Vector2f nav_adjusted = nav_vector - wind2d;
nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
}
}
// return a ground speed estimate in m/s
Vector2f AP_AHRS::groundspeed_vector(void)
{
@ -173,7 +154,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
Vector2f gndVelADS;
Vector2f gndVelGPS;
float airspeed;
bool gotAirspeed = airspeed_estimate(&airspeed);
bool gotAirspeed = airspeed_estimate_true(&airspeed);
bool gotGPS = (_gps && _gps->status() >= GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
Vector3f wind = wind_estimate();

36
libraries/AP_AHRS/AP_AHRS.h
View File

@ -150,6 +150,24 @@ public:
// if we have an estimate
virtual bool airspeed_estimate(float *airspeed_ret);
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool airspeed_estimate_true(float *airspeed_ret) {
if (!airspeed_estimate(airspeed_ret)) {
return false;
}
*airspeed_ret *= get_EAS2TAS();
return true;
}
// get apparent to true airspeed ratio
float get_EAS2TAS(void) const {
if (_airspeed) {
return _airspeed->get_EAS2TAS();
}
return 1.0f;
}
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(void) const {
@ -162,9 +180,6 @@ public:
// return true if we will use compass for yaw
virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }
// correct a bearing in centi-degrees for wind
void wind_correct_bearing(int32_t &nav_bearing_cd);
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return _flags.have_initial_yaw;
@ -190,21 +205,22 @@ public:
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
// settable parameters
AP_Float beta;
// for holding parameters
static const struct AP_Param::GroupInfo var_info[];
// these are public for ArduCopter
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
protected:
// settable parameters
AP_Float beta;
AP_Int8 _gps_use;
AP_Int8 _wind_max;
AP_Int8 _board_orientation;
AP_Int8 _gps_minsats;
// for holding parameters
static const struct AP_Param::GroupInfo var_info[];
protected:
// flags structure
struct ahrs_flags {
uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -711,7 +711,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
} else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) {
// when flying straight use airspeed to get wind estimate if available
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - airspeed;
Vector3f wind = velocity - (airspeed * get_EAS2TAS());
_wind = _wind * 0.92f + wind * 0.08f;
}
}
@ -799,9 +799,11 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
float gnd_speed = _gps->ground_speed_cm*0.01f;
*airspeed_ret = constrain_float(*airspeed_ret,
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,
gnd_speed + _wind_max);
*airspeed_ret = true_airspeed / get_EAS2TAS();
}
return ret;
}