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

FW Rate Controller: fix manual tailsitter (only transform to body frame once) (#21565) 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-10 09:29:02 +02:00 committed by GitHub
parent 8222cbca2c
commit ab71ef4447
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -120,24 +120,15 @@ FixedwingRateControl::vehicle_manual_poll()
_rate_sp_pub.publish(_rates_sp);
} else {
/* manual/direct control */
if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// the controls must always be published in body (hover) frame
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
} else {
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
}
// Manual/direct control, filled in FW-frame. Note that setpoints will get transformed to body frame prior publishing.
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() +
_param_trim_pitch.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
_vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f);
}
}