From 0211ef3ba1d22afab4806cdbed5c64b5e1a00f70 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 10 Nov 2021 21:28:32 +0100 Subject: [PATCH] FlightTaskAutoMapper: move update() into FlightTaskAutoLineSmooth --- .../FlightTaskAutoLineSmoothVel.cpp | 64 +++++++++++++++++++ .../FlightTaskAutoLineSmoothVel.hpp | 3 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 64 ------------------- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 6 +- 4 files changed, 67 insertions(+), 70 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 35a3eb9b54..6816b43053 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -110,6 +110,70 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi) _yaw_sp_prev += delta_psi; } +bool FlightTaskAutoLineSmoothVel::update() +{ + bool ret = FlightTaskAuto::update(); + // always reset constraints because they might change depending on the type + _setDefaultConstraints(); + + // 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. + + if (_type_previous == WaypointType::idle) { + _acceleration_setpoint.setNaN(); + } + + // during mission and reposition, raise the landing gears but only + // if altitude is high enough + if (_highEnoughForLandingGear()) { + _gear.landing_gear = landing_gear_s::GEAR_UP; + } + + switch (_type) { + case WaypointType::idle: + _prepareIdleSetpoints(); + break; + + case WaypointType::land: + _prepareLandSetpoints(); + break; + + case WaypointType::loiter: + + /* fallthrought */ + case WaypointType::position: + _preparePositionSetpoints(); + break; + + case WaypointType::takeoff: + _prepareTakeoffSetpoints(); + break; + + case WaypointType::velocity: + _prepareVelocitySetpoints(); + break; + + default: + _preparePositionSetpoints(); + break; + } + + if (_param_com_obs_avoid.get()) { + _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); + _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, + _yawspeed_setpoint); + } + + + _generateSetpoints(); + + // update previous type + _type_previous = _type; + + return ret; +} + void FlightTaskAutoLineSmoothVel::_generateSetpoints() { _checkEmergencyBraking(); diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index ece4e8bbf1..a9de21d9dc 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -51,6 +51,7 @@ public: bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; void reActivate() override; + bool update() override; private: PositionSmoothing _position_smoothing; @@ -65,7 +66,7 @@ protected: void _ekfResetHandlerVelocityZ(float delta_vz) override; void _ekfResetHandlerHeading(float delta_psi) override; - void _generateSetpoints() override; /**< Generate setpoints along line. */ + void _generateSetpoints(); /**< Generate setpoints along line. */ void _generateHeading(); void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 37c18f144a..4de1d1f69b 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -45,70 +45,6 @@ FlightTaskAutoMapper::FlightTaskAutoMapper() : _stick_acceleration_xy(this) {} -bool FlightTaskAutoMapper::update() -{ - bool ret = FlightTaskAuto::update(); - // always reset constraints because they might change depending on the type - _setDefaultConstraints(); - - // 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. - - if (_type_previous == WaypointType::idle) { - _acceleration_setpoint.setNaN(); - } - - // during mission and reposition, raise the landing gears but only - // if altitude is high enough - if (_highEnoughForLandingGear()) { - _gear.landing_gear = landing_gear_s::GEAR_UP; - } - - switch (_type) { - case WaypointType::idle: - _prepareIdleSetpoints(); - break; - - case WaypointType::land: - _prepareLandSetpoints(); - break; - - case WaypointType::loiter: - - /* fallthrought */ - case WaypointType::position: - _preparePositionSetpoints(); - break; - - case WaypointType::takeoff: - _prepareTakeoffSetpoints(); - break; - - case WaypointType::velocity: - _prepareVelocitySetpoints(); - break; - - default: - _preparePositionSetpoints(); - break; - } - - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - - - _generateSetpoints(); - - // update previous type - _type_previous = _type; - - return ret; -} - void FlightTaskAutoMapper::_prepareIdleSetpoints() { // Send zero thrust setpoint diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 2268bbaad8..e157feb19d 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -50,12 +50,8 @@ class FlightTaskAutoMapper : public FlightTaskAuto public: FlightTaskAutoMapper(); virtual ~FlightTaskAutoMapper() = default; - bool update() override; protected: - - virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */ - void _prepareIdleSetpoints(); void _prepareLandSetpoints(); void _prepareVelocitySetpoints(); @@ -77,7 +73,7 @@ protected: (ParamFloat) _param_mpc_man_y_max ); -private: +protected: Sticks _sticks; StickAccelerationXY _stick_acceleration_xy; StickYaw _stick_yaw;