fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: remove offtrack logic

Browse Source 
It was added at a time where the triplet target was directly fed as position setpoint to the controller.

Since the smoothing improvements to FlightTaskAuto(SmoothVel) and removing the previous "aggressive" FlightTaskAuto variant there should be no need anymore for this logic. It can sometimes lead to unexpected sideffects. E.g. the vehicle would suddenly change direction when exceeding some arbitrary threshold.
This commit is contained in:
Matthias Grob 2025-11-12 11:43:19 -05:00
parent 2e651117e8
commit d0004403a3
5 changed files with 9 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld
View File

@ -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)

1
boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld
View File

@ -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)

1
boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld
View File

@ -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)

90
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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)) {

11
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -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<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _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 */
};