diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-25 10:51:13 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-25 10:51:13 +0100 |
| commit | 8b0125fc3fe878d390d9ebdcfceade9ae191681b (patch) | |
| tree | b33c05ebbe10006f5ef56cb27b5e2cc9b9624d19 /src/modules/fw_pos_control_l1/landingslope.cpp | |
| parent | 5c33aeeb430a984d0802f1af73063afca793a98a (diff) | |
| download | px4-firmware-8b0125fc3fe878d390d9ebdcfceade9ae191681b.tar.gz px4-firmware-8b0125fc3fe878d390d9ebdcfceade9ae191681b.tar.bz2 px4-firmware-8b0125fc3fe878d390d9ebdcfceade9ae191681b.zip | |
fw landing: move more functionality to the landingslope class
Diffstat (limited to 'src/modules/fw_pos_control_l1/landingslope.cpp')
| -rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.cpp | 6 |
1 files changed, 6 insertions, 0 deletions
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index ecf51b740..a2d8525b9 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -47,6 +47,7 @@ #include <errno.h> #include <math.h> #include <unistd.h> +#include <mathlib/mathlib.h> void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, @@ -76,3 +77,8 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } +float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude) +{ + return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + |