Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 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 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_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; 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, _att_sp.pitch_body = constrain(_att_sp.pitch_body,
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); -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); _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)); 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_status_sub.update(&_vehicle_status);
vehicle_control_mode_poll(); vehicle_control_mode_poll();
vehicle_manual_poll(); vehicle_manual_poll(euler_angles.psi());
vehicle_land_detected_poll(); vehicle_land_detected_poll();
// the position controller will not emit attitude setpoints in some modes // 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(); int parameters_update();
void vehicle_control_mode_poll(); void vehicle_control_mode_poll();
void vehicle_manual_poll(); void vehicle_manual_poll(const float yaw_body);
void vehicle_attitude_setpoint_poll(); void vehicle_attitude_setpoint_poll();
void vehicle_rates_setpoint_poll(); void vehicle_rates_setpoint_poll();
void vehicle_land_detected_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 /* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */ * level out without new manual input */
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _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; _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); _manual_height_rate_setpoint_m_s);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _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 */ /* Copy thrust and pitch values from tecs */
if (_landed) { if (_landed) {
@@ -1800,7 +1800,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
} }
_att_sp.roll_body = roll_sp_new; _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 */ /* Copy thrust and pitch values from tecs */