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

Merged release into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 11:06:01 +02:00
parent 2fc069bd63 a0176474c7
commit c9fefe236b
47 changed files with 1036 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+34 -11
View File
@@ -859,11 +859,36 @@ FixedwingAttitudeControl::task_main()
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
@@ -890,10 +915,8 @@ FixedwingAttitudeControl::task_main()
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
@@ -1060,9 +1083,9 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[0] = _manual.y + _parameters.trim_roll;
_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
_actuators.control[2] = _manual.r + _parameters.trim_yaw;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
@@ -1079,8 +1102,8 @@ FixedwingAttitudeControl::task_main()
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
{
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {