From 7686533abbdae7168248ddfeccf503c5ced4ba15 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 9 Feb 2021 18:23:42 +0100 Subject: [PATCH] FlightTaskManualAcceleration: adapt position reset to new structure The mapping itself was seprated out into a calls because it was reused for the experimental nudging implementation. The position resets which were handled correctly before now change the wrong setpoints and I adjusted. The nudging has to be before any filtering, then these member setpoints which are essentially copies are not needed anymore. --- .../FlightTaskManualAcceleration.cpp | 31 ++----------------- .../FlightTaskManualAcceleration.hpp | 3 -- 2 files changed, 2 insertions(+), 32 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 9f0e09d004..fe6230620a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -79,37 +79,10 @@ bool FlightTaskManualAcceleration::update() void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY() { - if (PX4_ISFINITE(_position_setpoint(0))) { - _position_setpoint(0) = _position(0); - _position_setpoint(1) = _position(1); - } + _stick_acceleration_xy.resetPosition(); } void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY() { - if (PX4_ISFINITE(_velocity_setpoint(0))) { - _velocity_setpoint(0) = _velocity(0); - _velocity_setpoint(1) = _velocity(1); - } -} - -void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ() -{ - if (PX4_ISFINITE(_position_setpoint(2))) { - _position_setpoint(2) = _position(2); - } -} - -void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ() -{ - if (PX4_ISFINITE(_velocity_setpoint(2))) { - _velocity_setpoint(2) = _velocity(2); - } -} - -void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi) -{ - if (PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint += delta_psi; - } + _stick_acceleration_xy.resetVelocity(Vector2f(_velocity)); } diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 2073de6593..36d360ca81 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -58,7 +58,4 @@ private: void _ekfResetHandlerPositionXY() override; void _ekfResetHandlerVelocityXY() override; - void _ekfResetHandlerPositionZ() override; - void _ekfResetHandlerVelocityZ() override; - void _ekfResetHandlerHeading(float delta_psi) override; };