diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index c5c1a45922..c5d6d07401 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -57,7 +57,7 @@ enum class WaypointType : int { takeoff, land, idle, - offboard, // only part of this structure due to legacy reason. It is not used + offboard, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks follow_target }; @@ -105,13 +105,16 @@ protected: private: - matrix::Vector2f _lock_position_xy{NAN, NAN}; + matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ uORB::Subscription *_sub_triplet_setpoint{nullptr}; - matrix::Vector3f _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */ - matrix::Vector3f _triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/ - matrix::Vector3f _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ + matrix::Vector3f + _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */ + matrix::Vector3f + _triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/ + matrix::Vector3f + _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 32796930da..e46f601706 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -40,19 +40,13 @@ using namespace matrix; -#define SIGMA_SINGLE_OP 0.000001f -#define SIGMA_NORM 0.001f +static constexpr float SIGMA_SINGLE_OP = 0.000001f; +static constexpr float SIGMA_NORM = 0.001f; void FlightTaskAutoLine::_generateSetpoints() { _generateAltitudeSetpoints(); _generateXYsetpoints(); - - // during mission and reposition, raise the landing gears but only - // if altitude is high enough - if (_highEnoughForLandingGear()) { - _constraints.landing_gear = vehicle_constraints_s::GEAR_UP; - } } void FlightTaskAutoLine::_generateXYsetpoints() @@ -237,11 +231,4 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints() _velocity_setpoint(2) = 0.0f; _position_setpoint(2) = _target(2); } -} - - -bool FlightTaskAutoLine::_highEnoughForLandingGear() -{ - // return true if altitude is above two meters - return _alt_above_ground > 2.0f; -} +} \ No newline at end of file diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index dd7f669263..10a57e3ad6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -57,13 +57,8 @@ protected: (ParamFloat) MPC_ACC_DOWN_MAX ); - void _generateSetpoints() override; + void _generateSetpoints() override; /**< Generate setpoints along line. */ void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ - -private: - - bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ - }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp index e943039784..630e858ac6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp @@ -40,8 +40,6 @@ using namespace matrix; - - bool FlightTaskAutoMapper::activate() { bool ret = FlightTaskAuto::activate(); @@ -88,6 +86,12 @@ bool FlightTaskAutoMapper::update() _generateVelocitySetpoints(); } + // during mission and reposition, raise the landing gears but only + // if altitude is high enough + if (_highEnoughForLandingGear()) { + _constraints.landing_gear = vehicle_constraints_s::GEAR_UP; + } + // update previous type _type_previous = _type; @@ -165,3 +169,9 @@ void FlightTaskAutoMapper::updateParams() // make sure that alt1 is above alt2 MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get())); } + +bool FlightTaskAutoMapper::_highEnoughForLandingGear() +{ + // return true if altitude is above two meters + return _alt_above_ground > 2.0f; +} \ No newline at end of file diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp index 8fddf5b7ea..f89a424087 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp @@ -42,7 +42,6 @@ #include "FlightTaskAuto.hpp" - class FlightTaskAutoMapper : public FlightTaskAuto { public: @@ -76,5 +75,5 @@ protected: private: void _reset(); /**< Resets member variables to current vehicle state */ WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ - + bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ };