fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 23:23:33 +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
co-authored by Matthias Grob
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/navigator/mission.cpp
+2 -15
View File
@@ -322,8 +322,7 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_mission_item.lat = _global_pos_sub.get().lat;
_mission_item.lon = _global_pos_sub.get().lon;
/* hold heading for takeoff items */
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly
_mission_item.altitude = _mission_init_climb_altitude_amsl;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
@@ -366,9 +365,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_land_detected_sub.get().landed) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.disable_weather_vane = true;
/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_global_pos_sub.get().lat, _global_pos_sub.get().lon,
@@ -389,16 +385,13 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_land_detected_sub.get().landed) {
/* re-enable weather vane again after alignment */
pos_sp_triplet->current.disable_weather_vane = false;
/* check if the vtol_takeoff waypoint is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
}
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.yaw = NAN;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -428,9 +421,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
&& !_land_detected_sub.get().landed
&& (num_found_items > 0u)) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.disable_weather_vane = true;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING;
set_align_mission_item(&_mission_item, &next_mission_items[0u]);
@@ -445,9 +435,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT;
/* re-enable weather vane again after alignment */
pos_sp_triplet->current.disable_weather_vane = false;
pos_sp_triplet->previous = pos_sp_triplet->current;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;