Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

FW Position controller: runway takeoff: track initial takeoff yaw if not in Mission Takeoff

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-11-09 16:50:47 +01:00
parent 1b89f26691
commit 7fb70c55df
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+9 -2
View File
@@ -1428,6 +1428,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_takeoff_ground_alt = _current_altitude; _takeoff_ground_alt = _current_altitude;
_launch_current_yaw = _yaw;
mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t"); mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t");
events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway");
} }
@@ -1459,8 +1461,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_runway_takeoff.getStartPosition()(1)); _runway_takeoff.getStartPosition()(1));
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP
const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)};
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
}
if (_param_fw_use_npfg.get()) { if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedNom(target_airspeed * _eas2tas);