From 5ea8c6e507da2770100181d078b1d082ab503dfa Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 16:50:53 +0200 Subject: [PATCH] FlightTaskAuto: remove unused _getTargetVelocityXY() Inclusive velocity_valid field in position_setpoint message that's then no longer needed. Signed-off-by: Silvan Fuhrer --- msg/position_setpoint.msg | 1 - .../tasks/Auto/FlightTaskAuto.cpp | 17 ----------------- .../tasks/Auto/FlightTaskAuto.hpp | 1 - src/modules/navigator/loiter.cpp | 2 -- src/modules/navigator/mission.cpp | 1 - 5 files changed, 22 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index b52f03a922..2323a67b65 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -15,7 +15,6 @@ uint8 type # setpoint type to adjust behavior of position controller float32 vx # local velocity setpoint in m/s in NED float32 vy # local velocity setpoint in m/s in NED float32 vz # local velocity setpoint in m/s in NED -bool velocity_valid # true if local velocity setpoint valid float64 lat # latitude, in deg float64 lon # longitude, in deg diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 9dc43e562d..440a112a1a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -599,23 +599,6 @@ bool FlightTaskAuto::_evaluateGlobalReference() return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); } -Vector2f FlightTaskAuto::_getTargetVelocityXY() -{ - // guard against any bad velocity values - const float vx = _sub_triplet_setpoint.get().current.vx; - const float vy = _sub_triplet_setpoint.get().current.vy; - bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && - _sub_triplet_setpoint.get().current.velocity_valid; - - if (velocity_valid) { - return Vector2f(vx, vy); - - } else { - // just return zero speed - return Vector2f{}; - } -} - State FlightTaskAuto::_getCurrentState() { // Calculate the vehicle current state based on the Navigator triplets and the current position. diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 302a1d66f6..167b141f2a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -94,7 +94,6 @@ public: void overrideCruiseSpeed(const float cruise_speed_m_s) override; protected: - matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 0d1808f13b..07dfc5f3f7 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -121,7 +121,6 @@ Loiter::set_loiter_position() } // convert mission item to current setpoint - pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->previous.valid = false; mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -146,7 +145,6 @@ Loiter::reposition() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading; pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat; pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0f22467c53..5d6c54c6f5 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1882,7 +1882,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (p1->velocity_valid == p2->velocity_valid) && (fabs(p1->lat - p2->lat) < DBL_EPSILON) && (fabs(p1->lon - p2->lon) < DBL_EPSILON) && (fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&