dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW Position control: use yaw at launch detection as bearing setpoint during hand launch takeoff 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-11-07 12:08:25 +01:00
parent b2f21b956c
commit 59e5c68cb0
2 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -1550,14 +1550,20 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_launch_detected = true;
_launch_global_position = global_position;
_takeoff_ground_alt = _current_altitude;
_launch_current_yaw = _yaw;
}
const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0),
_launch_global_position(1));
const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
// by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP
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 launch to the takeoff waypoint is followed until the clearance altitude is exceeded
takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
}
/* Set control values depending on the detection state */
if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH
@ -2596,7 +2602,7 @@ FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_p
// takeoff in the direction of the airframe
takeoff_bearing_vector(0) = cosf(_yaw);
takeoff_bearing_vector(0) = sinf(_yaw);
takeoff_bearing_vector(1) = sinf(_yaw);
}
return takeoff_bearing_vector;

3
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -315,6 +315,9 @@ private:
// [deg] global position of the vehicle at the time launch is detected (using launch detector)
Vector2d _launch_global_position{0, 0};
// [rad] current vehicle yaw at the time the launch is detected
float _launch_current_yaw{0.f};
// class handling runway takeoff for fixed-wing UAVs with steerable wheels
RunwayTakeoff _runway_takeoff;