diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 53c5e83ef6..4bce1a28eb 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -44,6 +44,7 @@ add_subdirectory(conversion) add_subdirectory(drivers) add_subdirectory(ecl) add_subdirectory(FlightTasks) +add_subdirectory(landing_slope) add_subdirectory(led) add_subdirectory(mathlib) add_subdirectory(mixer) diff --git a/src/lib/landing_slope/CMakeLists.txt b/src/lib/landing_slope/CMakeLists.txt new file mode 100644 index 0000000000..55f5940b78 --- /dev/null +++ b/src/lib/landing_slope/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(landing_slope Landingslope.cpp) diff --git a/src/modules/fw_pos_control_l1/Landingslope.cpp b/src/lib/landing_slope/Landingslope.cpp similarity index 74% rename from src/modules/fw_pos_control_l1/Landingslope.cpp rename to src/lib/landing_slope/Landingslope.cpp index a06130ed75..2e14cf57ab 100644 --- a/src/modules/fw_pos_control_l1/Landingslope.cpp +++ b/src/lib/landing_slope/Landingslope.cpp @@ -83,7 +83,6 @@ Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, flo } return 0.0f; - } float @@ -97,5 +96,36 @@ Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float } return 0.0f; - +} + +/** + * + * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, + float landing_slope_angle_rad) +{ + // flare_relative_alt is negative + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); +} + +/** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad) +{ + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, + landing_slope_angle_rad) + wp_landing_altitude; +} + +/** + * + * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude + */ +float Landingslope::getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad) +{ + return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement; } diff --git a/src/modules/fw_pos_control_l1/Landingslope.hpp b/src/lib/landing_slope/Landingslope.hpp similarity index 82% rename from src/modules/fw_pos_control_l1/Landingslope.hpp rename to src/lib/landing_slope/Landingslope.hpp index e6a286c54b..6bdda717a1 100644 --- a/src/modules/fw_pos_control_l1/Landingslope.hpp +++ b/src/lib/landing_slope/Landingslope.hpp @@ -80,34 +80,22 @@ public: * * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, - float landing_slope_angle_rad) - { - // flare_relative_alt is negative - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); - } + static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, + float landing_slope_angle_rad); /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad) - { - return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, - landing_slope_angle_rad) + wp_landing_altitude; - } + static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad); /** * * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude */ - __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad) - { - return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement; - - } + static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, + float horizontal_slope_displacement, float landing_slope_angle_rad); float getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 210863e394..21d0b666ee 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -40,11 +40,11 @@ px4_add_module( STACK_MAIN 1200 SRCS FixedwingPositionControl.cpp - Landingslope.cpp DEPENDS git_ecl ecl_l1 ecl_tecs launchdetection + landing_slope runway_takeoff ) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index cf1dc40c4d..bd52f62e7b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -48,7 +48,6 @@ #ifndef FIXEDWINGPOSITIONCONTROL_HPP_ #define FIXEDWINGPOSITIONCONTROL_HPP_ -#include "Landingslope.hpp" #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" @@ -58,6 +57,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index ce79d1cb66..a11c4de27e 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -56,4 +56,5 @@ px4_add_module( DEPENDS git_ecl ecl_geo + landing_slope ) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dfa3baa29d..1495b5e29a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -45,9 +45,9 @@ #include "navigator.h" #include -#include #include #include +#include #include #include #include