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

Navigator/FlightTaskAuto yaw handling improvements/simplifications (#22532)

Browse Source 
* PositionSetpoint: remove yaw_valid field

* Navigator: set yaw setpoint to NAN for Takeoff

Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
The yaw setpoint generation is handled by FlightTaskAuto.

* PositionSetpoint.msg: remove disable_weather_vane and instead only use the yaw field

Strictly follow the concept that if the position_setpoint.yaw is set, then
follow it the controller, and otherwise let the controller set it as it
thinks it's best.

* Navigator: remove logic that sets yaw to be accepted in TAKEOFF

No longer needed as during Takeoff we anyway don't set a yaw setpoint.

* PositionSetpoint.msg: remove yawspeed_valid

* PositionSetpoint.msg: remove yawspeed

* Navigator: set yaw setpoint to NAN instead of current

In set_takeoff and set_land_item, as well as for VTOL transition.
The flight tasks then set the yaw corresponding to the current yaw.

* Navigator: change get_yaw_acceptance into a bool

* PositionSetpoint.msg: improve comment for yaw

* MissionBlock: remove unnecessary code from set_vtol_transition_item

* Navigator: clean up calculate_breaking_stop(), set yaw to NAN

* Navigator: set yaw to NAN in variouls places where not specifc setpoint is desired

* Navigator: set yaw to NAN in reset_position_setpoint()

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Silvan Fuhrer
2023-12-21 16:50:13 +01:00
committed by GitHub
parent d872ef87da
commit 7e22b47b85
9 changed files with 38 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+3 -9
View File
@@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets()
}
// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_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;
@@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint.get().next.yaw,
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
(float)NAN,
_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(
_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.f;
} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
&& _sub_triplet_setpoint.get().current.yaw_valid) {
// Use the yaw computed in Navigator except during takeoff because
// Navigator is not handling the yaw reset properly.
// But: use if from Navigator during takeoff if disable_weather_vane is true,
// because we're then aligning to the transition waypoint.
// TODO: fix in navigator
} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
_yawspeed_setpoint = NAN;