From ec73fadc45052db0837b1af7ed437c84c09c9abf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2013 11:11:57 +1000 Subject: [PATCH] AP_AHRS: added true airspeed support in AHRS use true airspeed for wind calculations, and allow other drivers to ask for the current ratio --- libraries/AP_AHRS/AP_AHRS.cpp | 27 ++++------------------- libraries/AP_AHRS/AP_AHRS.h | 36 ++++++++++++++++++++++--------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 6 ++++-- 3 files changed, 34 insertions(+), 35 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 5312cd4bfe..f921b744bc 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -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(); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 99af4b09c6..04a1e2e9b3 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 863705c86a..e8f033476d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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; }