Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: move wind helper functions to AP_AHRS

This commit is contained in:
Tom Pittenger 2023-12-19 15:55:00 -08:00 committed by Andrew Tridgell
parent 813c732a83
commit 7a9c6eea9c
3 changed files with 1 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Landing/AP_Landing.cpp
View File

@ -548,33 +548,6 @@ bool AP_Landing::get_target_altitude_location(Location &location)
}
}
/*
* Determine how aligned heading_deg is with the wind. Return result
* is 1.0 when perfectly aligned heading into wind, -1 when perfectly
* aligned with-wind, and zero when perfect cross-wind. There is no
* distinction between a left or right cross-wind. Wind speed is ignored
*/
float AP_Landing::wind_alignment(const float heading_deg)
{
const Vector3f wind = ahrs.wind_estimate();
const float wind_heading_rad = atan2f(-wind.y, -wind.x);
return cosf(wind_heading_rad - radians(heading_deg));
}
/*
* returns head-wind in m/s, 0 for tail-wind.
*/
float AP_Landing::head_wind(void)
{
const float alignment = wind_alignment(ahrs.yaw_sensor*0.01f);
if (alignment <= 0) {
return 0;
}
return alignment * ahrs.wind_estimate().length();
}
/*
* returns target airspeed in cm/s depending on flight stage
*/

2
libraries/AP_Landing/AP_Landing.h
View File

@ -91,8 +91,6 @@ public:
// helper functions
bool restart_landing_sequence(void);
float wind_alignment(const float heading_deg);
float head_wind(void);
int32_t get_target_airspeed_cm(void);
// accessor functions for the params and states

2
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -368,7 +368,7 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void)
// when landing, add half of head-wind.
const float head_wind_comp = constrain_float(wind_comp, 0.0f, 100.0f)*0.01;
const int32_t head_wind_compensation_cm = head_wind() * head_wind_comp * 100;
const int32_t head_wind_compensation_cm = ahrs.head_wind() * head_wind_comp * 100;
const uint32_t max_airspeed_cm = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max*100 : aparm.airspeed_cruise_cm;