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

Navigator: VTOL: set heading of transition of a VTOL_TAKEOFF to the VTOL_TAKEOFF waypoint (and enforce heading)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-08-21 10:08:04 +02:00
committed by Roman Bapst
parent b1d38ee050
commit 8a8cc3b52b
1 changed files with 20 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+20
View File
@@ -758,6 +758,26 @@ Mission::set_mission_items()
!_navigator->get_land_detected()->landed) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.allow_weather_vane = false;
/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
_mission_item.force_heading = true;
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
/* set position setpoint to current while aligning */
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
}
/* heading is aligned now, prepare transition */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
_work_item_type == WORK_ITEM_TYPE_ALIGN &&
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_navigator->get_land_detected()->landed) {
/* check if the vtol_takeoff waypoint is on top of us */