From 04cd247c906bc34a4869a7cc2b1be45a85018f47 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:34:48 +0100 Subject: [PATCH] FlightTaskAuto: remove isTargetModified() As it is no longer needed w/o avoidance. Signed-off-by: Silvan --- .../tasks/Auto/FlightTaskAuto.cpp | 14 -------------- .../tasks/Auto/FlightTaskAuto.hpp | 1 - 2 files changed, 15 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 614bcc6118..419f25827a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -159,11 +159,6 @@ bool FlightTaskAuto::update() _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints - waypoints[2] = _position_setpoint; - } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; @@ -764,15 +759,6 @@ bool FlightTaskAuto::_generateHeadingAlongTraj() return res; } -bool FlightTaskAuto::isTargetModified() const -{ - const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); - const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; - - return xy_modified || z_modified; -} - void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 6c49dd8726..5b8d3e9b5b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -108,7 +108,6 @@ protected: void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - bool isTargetModified() const; void _updateTrajConstraints(); void rcHelpModifyYaw(float &yaw_sp);