Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

fixed-wing: set yaw_sp to yaw_current instead of 0 when not controlled

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-22 10:41:39 +02:00
committed by Roman Bapst
parent da4d6dc657
commit b53808d11b
3 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+3 -3
View File
@@ -134,7 +134,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
}
void
FixedwingAttitudeControl::vehicle_manual_poll()
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
{
const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
@@ -156,7 +156,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = 0.0f;
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
@@ -357,7 +357,7 @@ void FixedwingAttitudeControl::Run()
_vehicle_status_sub.update(&_vehicle_status);
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_manual_poll(euler_angles.psi());
vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+1 -1
View File
@@ -233,7 +233,7 @@ private:
int parameters_update();
void vehicle_control_mode_poll();
void vehicle_manual_poll();
void vehicle_manual_poll(const float yaw_body);
void vehicle_attitude_setpoint_poll();
void vehicle_rates_setpoint_poll();
void vehicle_land_detected_poll();
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+3 -3
View File
@@ -693,7 +693,7 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
/* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1681,7 +1681,7 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
_manual_height_rate_setpoint_m_s);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
/* Copy thrust and pitch values from tecs */
if (_landed) {
@@ -1800,7 +1800,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
}
_att_sp.roll_body = roll_sp_new;
_att_sp.yaw_body = 0;
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
/* Copy thrust and pitch values from tecs */