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

Fixed-wing position control: set yaw_sp to yaw_current instead of nav_bearing when not controlled 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-22 10:45:51 +02:00 committed by Roman Bapst
parent b53808d11b
commit e715e6c245
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -686,7 +686,7 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
@ -1047,7 +1047,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
tecs_update_pitch_throttle(now, position_sp_alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
@ -1151,7 +1151,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
float alt_sp = pos_sp_curr.alt;
@ -1266,7 +1266,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
// assign values
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
@ -1307,7 +1307,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
* full throttle, otherwise we use idle throttle */
@ -1464,7 +1464,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
}
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
@ -1775,7 +1775,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */