fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

landing_slope: added wrap_pi to getLandingSlope and getFlareCurve

Browse Source 
- fixes https://github.com/PX4/PX4-Autopilot/issues/16132
This commit is contained in:
AlexanderAurora 2020-11-10 18:58:14 -05:00 committed by GitHub
parent f4964aea52
commit ea09d9ebbf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/landing_slope/Landingslope.cpp
View File

@ -40,6 +40,7 @@
#include "Landingslope.hpp"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
void
Landingslope::update(float landing_slope_angle_rad_new,
@ -77,7 +78,7 @@ Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, flo
float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
return getLandingSlopeRelativeAltitude(wp_landing_distance);
}
@ -90,7 +91,7 @@ Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float
float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;
}