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

FW control: Add skeleton for distinct altitude control and position control flight modes

Browse Source 
Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Lorenz Meier
2015-06-05 22:45:05 +02:00
parent 8e935e6fa6
commit 5197be67a7
2 changed files with 167 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+26
View File
@@ -854,6 +854,32 @@ 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) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _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.
*/