fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW attitude control: fix yaw rate publishing

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-08-12 18:22:14 +02:00
parent 6724e4af26
commit dadd4f39ad
2 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -579,14 +579,14 @@ void FixedwingAttitudeControl::Run()
&& ((now - pid_autotune.timestamp) < 1_s)) {
bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp);
body_rates_setpoint = body_rates_setpoint + bodyrate_autotune_ff;
body_rates_setpoint += bodyrate_autotune_ff;
}
}
/* add yaw rate setpoint from sticks in Stabilized mode */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()),
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_y_rmax.get()),
-radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
}
@ -640,9 +640,7 @@ void FixedwingAttitudeControl::Run()
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
/*
* Publish the rate setpoint for analysis once available
*/
/* Publish the rate setpoint for analysis once available */
_rates_sp.roll = body_rates_setpoint(0);
_rates_sp.pitch = body_rates_setpoint(1);
_rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_body_rate_setpoint() : body_rates_setpoint(2);

3
src/modules/fw_att_control/ecl_yaw_controller.h
View File

@ -60,9 +60,6 @@ public:
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
protected:
float _max_rate{0.0f};
};
#endif // ECL_YAW_CONTROLLER_H