// return a ground speed estimate in m/s
Vector2f BC_AHRS::groundspeed_vector(void)
{
// Generate estimate of ground speed vector using air data system
Vector2f gndVelADS;
Vector2f gndVelGPS;
float airspeed;
bool gotAirspeed = airspeed_estimate_true(&airspeed);
bool gotGPS = (_gps.status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
Vector3f wind = wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
Vector2f airspeed_vector = Vector2f(cosf(yaw), sinf(yaw)) * airspeed;
gndVelADS = airspeed_vector - wind2d;
}
// Generate estimate of ground speed vector using GPS
if (gotGPS) {
float cog = radians(_gps.ground_course_cd()*0.01f);
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed();
}
// If both ADS and GPS data is available, apply a complementary filter
if (gotAirspeed && gotGPS) {
// The LPF is applied to the GPS and the HPF is applied to the air data estimate
// before the two are summed
//Define filter coefficients
// alpha and beta must sum to one
// beta = dt/Tau, where
// dt = filter time step (0.1 sec if called by nav loop)
// Tau = cross-over time constant (nominal 2 seconds)
// More lag on GPS requires Tau to be bigger, less lag allows it to be smaller
// To-Do - set Tau as a function of GPS lag.
const float alpha = 1.0f - beta;
// Run LP filters
_lp = gndVelGPS * beta + _lp * alpha;
// Run HP filters
_hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
// Save the current ADS ground vector for the next time step
_lastGndVelADS = gndVelADS;
// Sum the HP and LP filter outputs
return _hp + _lp;
}
// Only ADS data is available return ADS estimate
if (gotAirspeed && !gotGPS) {
return gndVelADS;
}
// Only GPS data is available so return GPS estimate
if (!gotAirspeed && gotGPS) {
return gndVelGPS;
}
return Vector2f(0.0f, 0.0f);
}
/*
* Update AOA and SSA estimation based on airspeed, velocity vector and wind vector
*
* Based on:
* "On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors" by
* Tor A. Johansen, Andrea Cristofaro, Kim Sorensen, Jakob M. Hansen, Thor I. Fossen
*
* "Multi-Stage Fusion Algorithm for Estimation of Aerodynamic Angles in Mini Aerial Vehicle" by
* C.Ramprasadh and Hemendra Arya
*
* "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by
* JOSEPH E. ZEIS, JR., CAPTAIN, USAF
*/
void AP_AHRS::update_AOA_SSA(void)
{
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
const uint32_t now = AP_HAL::millis();
if (now - _last_AOA_update_ms < 50) {
// don't update at more than 20Hz
return;
}
_last_AOA_update_ms = now;
Vector3f aoa_velocity, aoa_wind;
// get velocity and wind
if (get_velocity_NED(aoa_velocity) == false) {
return;
}
aoa_wind = wind_estimate();
// Rotate vectors to the body frame and calculate velocity and wind
const Matrix3f &rot = get_rotation_body_to_ned();
aoa_velocity = rot.mul_transpose(aoa_velocity);
aoa_wind = rot.mul_transpose(aoa_wind);
// calculate relative velocity in body coordinates
aoa_velocity = aoa_velocity - aoa_wind;
const float vel_len = aoa_velocity.length();
// do not calculate if speed is too low
if (vel_len < 2.0) {
_AOA = 0;
_SSA = 0;
return;
}
// Calculate AOA and SSA
if (aoa_velocity.x > 0) {
_AOA = degrees(atanf(aoa_velocity.z / aoa_velocity.x));
} else {
_AOA = 0;
}
_SSA = degrees(safe_asin(aoa_velocity.y / vel_len));
#endif
}
// 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;
}
}
