From 4d0786d5b45c30cc6e6a1b80525c510ddc3cf6e7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Nov 2019 09:44:51 +0100 Subject: [PATCH] FlightTask: use centralized _dist_to_ground computed in FlightTask instead of recomputing it in AutoMapper, AutoMapper2 and AutoLine --- .../tasks/AutoLine/FlightTaskAutoLine.cpp | 2 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 21 ++--------------- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 3 --- .../AutoMapper2/FlightTaskAutoMapper2.cpp | 23 +++---------------- .../AutoMapper2/FlightTaskAutoMapper2.hpp | 3 --- .../tasks/FlightTask/FlightTask.cpp | 4 ++-- 6 files changed, 8 insertions(+), 48 deletions(-) diff --git a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp index 16388ffcf0..2dde9449c9 100644 --- a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -214,7 +214,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints() // limit vertical downwards speed (positive z) close to ground // for now we use the altitude above home and assume that we want to land at same height as we took off - float vel_limit = math::gradual(_alt_above_ground, + float vel_limit = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), _param_mpc_land_speed.get(), _constraints.speed_down); diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 99d4a19799..b750f4379e 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -52,8 +52,6 @@ bool FlightTaskAutoMapper::update() // always reset constraints because they might change depending on the type _setDefaultConstraints(); - _updateAltitudeAboveGround(); - bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position; bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position; @@ -137,7 +135,7 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints() _velocity_setpoint = Vector3f(NAN, NAN, NAN); // limit vertical speed during takeoff - _constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(), + _constraints.speed_up = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up); _gear.landing_gear = landing_gear_s::GEAR_DOWN; @@ -152,21 +150,6 @@ void FlightTaskAutoMapper::_generateVelocitySetpoints() _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); } -void FlightTaskAutoMapper::_updateAltitudeAboveGround() -{ - // Altitude above ground is by default just the negation of the current local position in D-direction. - _alt_above_ground = -_position(2); - - if (PX4_ISFINITE(_dist_to_bottom)) { - // We have a valid distance to ground measurement - _alt_above_ground = _dist_to_bottom; - - } else if (_sub_home_position.get().valid_alt) { - // if home position is set, then altitude above ground is relative to the home position - _alt_above_ground = -_position(2) + _sub_home_position.get().z; - } -} - void FlightTaskAutoMapper::updateParams() { FlightTaskAuto::updateParams(); @@ -178,5 +161,5 @@ void FlightTaskAutoMapper::updateParams() bool FlightTaskAutoMapper::_highEnoughForLandingGear() { // return true if altitude is above two meters - return _alt_above_ground > 2.0f; + return _dist_to_ground > 2.0f; } diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index f15044129b..2a1cfbc299 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -59,11 +59,8 @@ protected: void _generateVelocitySetpoints(); void _generateTakeoffSetpoints(); - void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */ void updateParams() override; /**< See ModuleParam class */ - float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) _param_mpc_land_speed, (ParamFloat) _param_mpc_tiltmax_lnd, diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index 0f7fbb3a33..e9523ec640 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -52,8 +52,6 @@ bool FlightTaskAutoMapper2::update() // always reset constraints because they might change depending on the type _setDefaultConstraints(); - _updateAltitudeAboveGround(); - // The only time a thrust set-point is sent out is during // idle. Hence, reset thrust set-point to NAN in case the // vehicle exits idle. @@ -144,7 +142,7 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints() { // Takeoff is completely defined by target position _position_setpoint = _target; - const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up : + const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up : _param_mpc_tko_speed.get(); _velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed @@ -167,21 +165,6 @@ void FlightTaskAutoMapper2::_preparePositionSetpoints() _velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations } -void FlightTaskAutoMapper2::_updateAltitudeAboveGround() -{ - // Altitude above ground is by default just the negation of the current local position in D-direction. - _alt_above_ground = -_position(2); - - if (PX4_ISFINITE(_dist_to_bottom)) { - // We have a valid distance to ground measurement - _alt_above_ground = _dist_to_bottom; - - } else if (_sub_home_position.get().valid_alt) { - // if home position is set, then altitude above ground is relative to the home position - _alt_above_ground = -_position(2) + _sub_home_position.get().z; - } -} - void FlightTaskAutoMapper2::updateParams() { FlightTaskAuto::updateParams(); @@ -193,7 +176,7 @@ void FlightTaskAutoMapper2::updateParams() bool FlightTaskAutoMapper2::_highEnoughForLandingGear() { // return true if altitude is above two meters - return _alt_above_ground > 2.0f; + return _dist_to_ground > 2.0f; } float FlightTaskAutoMapper2::_getLandSpeed() @@ -209,7 +192,7 @@ float FlightTaskAutoMapper2::_getLandSpeed() float speed = 0; - if (_alt_above_ground > _param_mpc_land_alt1.get()) { + if (_dist_to_ground > _param_mpc_land_alt1.get()) { speed = _constraints.speed_down; } else { diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp index 15c15ca15a..212270b961 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp +++ b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp @@ -60,11 +60,8 @@ protected: void _prepareTakeoffSetpoints(); void _preparePositionSetpoints(); - void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */ void updateParams() override; /**< See ModuleParam class */ - float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) _param_mpc_land_speed, (ParamFloat) _param_mpc_tiltmax_lnd, diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index eb65d95598..58038977d3 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -164,9 +164,9 @@ void FlightTask::_evaluateVehicleLocalPosition() void FlightTask::_evaluateDistanceToGround() { - _dist_to_ground = NAN; + // Altitude above ground is by default just the negation of the current local position in D-direction. + _dist_to_ground = -_position(2); - // if there is a valid distance to bottom or vertical distance to home if (PX4_ISFINITE(_dist_to_bottom)) { _dist_to_ground = _dist_to_bottom;