From 1cef2ad196ee029e079159a1c397ac5bb00356c3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 10 Nov 2021 21:11:47 +0100 Subject: [PATCH] FlightTaskAutoMapper: merge activate() into FlightTaskAutoLineSmooth --- .../FlightTaskAutoLineSmoothVel.cpp | 6 +++++- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 14 -------------- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 2 -- 3 files changed, 5 insertions(+), 17 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 0306fb31b8..35a3eb9b54 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -42,7 +42,11 @@ using namespace matrix; bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) { - bool ret = FlightTaskAutoMapper::activate(last_setpoint); + bool ret = FlightTaskAuto::activate(last_setpoint); + + // Set setpoints equal current state. + _velocity_setpoint = _velocity; + _position_setpoint = _position; Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp index e67472314b..37c18f144a 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -45,13 +45,6 @@ FlightTaskAutoMapper::FlightTaskAutoMapper() : _stick_acceleration_xy(this) {} -bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskAuto::activate(last_setpoint); - _reset(); - return ret; -} - bool FlightTaskAutoMapper::update() { bool ret = FlightTaskAuto::update(); @@ -116,13 +109,6 @@ bool FlightTaskAutoMapper::update() return ret; } -void FlightTaskAutoMapper::_reset() -{ - // Set setpoints equal current state. - _velocity_setpoint = _velocity; - _position_setpoint = _position; -} - 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 e206274221..2268bbaad8 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -50,7 +50,6 @@ class FlightTaskAutoMapper : public FlightTaskAuto public: FlightTaskAutoMapper(); virtual ~FlightTaskAutoMapper() = default; - bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool update() override; protected: @@ -84,7 +83,6 @@ private: StickYaw _stick_yaw; matrix::Vector3f _land_position; float _land_heading; - 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. */ };