diff --git a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld index 36330c1e85..5662464ca3 100644 --- a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld @@ -573,7 +573,6 @@ *(.text._ZN4uORB12SubscriptionaSEOS0_) *(.text._ZN15TakeoffHandling18updateTakeoffStateEbbbfbRKy) *(.text._ZN10ModeChecks14checkAndReportERK7ContextR6Report) -*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) *(.text._ZN8Failsafe17updateArmingStateERKybRK16failsafe_flags_s) *(.text.imxrt_lpi2c_modifyreg) *(.text.up_flush_dcache) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld index 36330c1e85..5662464ca3 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -573,7 +573,6 @@ *(.text._ZN4uORB12SubscriptionaSEOS0_) *(.text._ZN15TakeoffHandling18updateTakeoffStateEbbbfbRKy) *(.text._ZN10ModeChecks14checkAndReportERK7ContextR6Report) -*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) *(.text._ZN8Failsafe17updateArmingStateERKybRK16failsafe_flags_s) *(.text.imxrt_lpi2c_modifyreg) *(.text.up_flush_dcache) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 4d676a6998..51e166ccbd 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -586,7 +586,6 @@ *(.text._ZN4uORB12SubscriptionaSEOS0_) *(.text._ZN15TakeoffHandling18updateTakeoffStateEbbbfbRKy) *(.text._ZN10ModeChecks14checkAndReportERK7ContextR6Report) -*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) *(.text._ZN8Failsafe17updateArmingStateERKybRK16failsafe_flags_s) *(.text.imxrt_lpi2c_modifyreg) *(.text.up_flush_dcache) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index ec999cdc0f..80c89a7675 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -357,7 +357,11 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = _yaw; _yawspeed_setpoint = NAN; _target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius; - _updateInternalWaypoints(); + + _prev_wp = _triplet_prev_wp; + _target = _triplet_target; + _next_wp = _triplet_next_wp; + return true; } @@ -409,7 +413,6 @@ bool FlightTaskAuto::_evaluateTriplets() // to the internal _triplet_target. // TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed. - bool triplet_update = true; const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid) || (_next_was_valid != _sub_triplet_setpoint.get().next.valid); @@ -419,7 +422,6 @@ bool FlightTaskAuto::_evaluateTriplets() && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f && !prev_next_validity_changed) { // Nothing has changed: just keep old waypoints. - triplet_update = false; } else { _triplet_target = tmp_target; @@ -462,19 +464,15 @@ bool FlightTaskAuto::_evaluateTriplets() } _next_was_valid = _sub_triplet_setpoint.get().next.valid; + + _prev_wp = _triplet_prev_wp; + _target = _triplet_target; + _next_wp = _triplet_next_wp; } // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) _weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)); - // Calculate the current vehicle state and check if it has updated. - State previous_state = _current_state; - _current_state = _getCurrentState(); - - if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) { - _updateInternalWaypoints(); - } - // set heading _weathervane.update(); @@ -610,76 +608,6 @@ bool FlightTaskAuto::_evaluateGlobalReference() return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); } -State FlightTaskAuto::_getCurrentState() -{ - // Calculate the vehicle current state based on the Navigator triplets and the current position. - const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero(); - const Vector3f prev_to_pos = _position - _triplet_prev_wp; - const Vector3f pos_to_target = _triplet_target - _position; - - // Calculate the closest point to the vehicle position on the line prev_wp - target - _closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target); - - State return_state = State::none; - - if (!u_prev_to_target.longerThan(FLT_EPSILON)) { - // Previous and target are the same point, so we better don't try to do any special line following - return_state = State::none; - - } else if (u_prev_to_target * pos_to_target < 0.0f) { - // Target is behind - return_state = State::target_behind; - - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) { - // Previous is in front - return_state = State::previous_infront; - - } else if (_type != WaypointType::land && (_position - _closest_pt).longerThan(_target_acceptance_radius)) { - // Vehicle too far from the track - return_state = State::offtrack; - } - - return return_state; -} - -void FlightTaskAuto::_updateInternalWaypoints() -{ - // The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp. - // The cases where it differs: - // 1. The vehicle already passed the target -> go straight to target - // 2. Previous waypoint is in front of the vehicle -> go straight to previous waypoint - // 3. The vehicle is far from track -> go straight to closest point on track - switch (_current_state) { - case State::target_behind: - _target = _triplet_target; - _prev_wp = _position; - _next_wp = _triplet_next_wp; - break; - - case State::previous_infront: - _next_wp = _triplet_target; - _target = _triplet_prev_wp; - _prev_wp = _position; - break; - - case State::offtrack: - _next_wp = _triplet_target; - _target = _closest_pt; - _prev_wp = _position; - break; - - case State::none: - _target = _triplet_target; - _prev_wp = _triplet_prev_wp; - _next_wp = _triplet_next_wp; - break; - - default: - break; - - } -} - bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) { if (PX4_ISFINITE(v.norm_squared()) && v.longerThan(1e-3f)) { diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 89dfc494c1..0166c7e28f 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -68,13 +68,6 @@ enum class WaypointType : int { idle = position_setpoint_s::SETPOINT_TYPE_IDLE }; -enum class State { - offtrack, /**< Vehicle is more than cruise speed away from track */ - target_behind, /**< Vehicle is in front of target. */ - previous_infront, /**< Vehilce is behind previous waypoint.*/ - none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ -}; - enum class yaw_mode : int32_t { towards_waypoint = 0, towards_home = 1, @@ -97,7 +90,6 @@ public: void overrideCruiseSpeed(const float cruise_speed_m_s) override; protected: - 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 */ /** Reset position or velocity setpoints in case of EKF reset event */ @@ -134,7 +126,6 @@ protected: uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; - State _current_state{State::none}; float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ float _yaw_setpoint_previous{NAN}; /**< Used because _yaw_setpoint is overwritten in multiple places */ @@ -191,7 +182,6 @@ private: _triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/ matrix::Vector3f _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/ - matrix::Vector3f _closest_pt; /**< closest point to the vehicle position on the line previous - target */ hrt_abstime _time_last_cruise_speed_override{0}; ///< timestamp the cruise speed was last time overridden using DO_CHANGE_SPEED @@ -207,6 +197,5 @@ private: bool _evaluateTriplets(); /**< Checks and sets triplets. */ bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ bool _evaluateGlobalReference(); /**< Check is global reference is available. */ - State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */ void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */ };