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

FWModeManager: use always bearing to Takeoff waypoint (if defined)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2025-07-10 13:57:12 +02:00
parent 782e510105
commit b326714d7c
2 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_mode_manager/FixedWingModeManager.cpp
+6 -4
View File
@@ -1180,18 +1180,19 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
_takeoff_init_position = global_position;
_takeoff_ground_alt = _current_altitude;
_launch_current_yaw = _yaw;
// _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); // TODO
}
const Vector2f launch_local_position = _global_local_proj_ref.project(_takeoff_init_position(0),
_takeoff_init_position(1));
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
// by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP
float takeoff_bearing = _launch_current_yaw;
const float distance_to_takeoff_wp = get_distance_to_next_waypoint(_takeoff_init_position(0), _takeoff_init_position(1),
pos_sp_curr.lat, pos_sp_curr.lon);
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && distance_to_takeoff_wp > 10.f) {
// if a takeoff waypoint is set (and not straight above current location), use the bearing to the takeoff waypoint
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - launch_local_position;
if (takeoff_bearing_vector.norm() > FLT_EPSILON) {
@@ -1206,6 +1207,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
const DirectionalGuidanceOutput sp = navigateLine(launch_local_position, takeoff_bearing, local_2D_position,
ground_speed,
_wind_vel);
fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
fw_lateral_ctrl_sp.timestamp = now;
fw_lateral_ctrl_sp.course = sp.course_setpoint;
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -640,7 +640,7 @@ void Navigator::run()
rep->next.valid = false;
// after the straight climbout the vehicle will establish on a loiter at this position
// Fixed-wing: vehicle will takeoff towards these coordinates and establish on a loiter there
_takeoff.setLoiterPosition(matrix::Vector2d(cmd.param5, cmd.param6));
_takeoff.setLoiterAltitudeAmsl(cmd.param7);