diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 6d86cdc8be..a63d136e7f 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -106,7 +106,6 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) if (!_is_position_on_circle()) { _in_circle_approach = true; _position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position); - _circle_approach_start_position = _position; } return ret; @@ -169,7 +168,6 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set && PX4_ISFINITE(_velocity(2)); _position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position); - _circle_approach_start_position = _position; return ret; } @@ -193,13 +191,6 @@ bool FlightTaskOrbit::update() _in_circle_approach = false; _altitude_velocity_smoothing.reset(0, _velocity(2), _position(2)); } - - } else { - if (!_in_circle_approach) { - _in_circle_approach = true; - _position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position); - _circle_approach_start_position = _position; - } } if (_in_circle_approach) { diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 96fa315913..54dcc640c4 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -118,7 +118,6 @@ private: matrix::Vector3f _center; /**< local frame coordinates of the center point */ bool _in_circle_approach = false; - Vector3f _circle_approach_start_position; PositionSmoothing _position_smoothing; VelocitySmoothing _altitude_velocity_smoothing; Vector3f _unsmoothed_velocity_setpoint;