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

fw position / fw attitude control: move attitude setpoint generation to

Browse Source 
position controller

- attitude setpoints for all modes are now computed in the fw position
controller

Signed-off-by: tumbili <roman@px4.io>
This commit is contained in:
tumbili
2016-07-25 08:56:18 +02:00
committed by Andreas Antener
parent e3f3233ee4
commit fd51bf44d5
2 changed files with 31 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+16 -118
View File
@@ -975,129 +975,27 @@ FixedwingAttitudeControl::task_main()
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_auto_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
yaw_sp = _att_sp.yaw_body;
throttle_sp = _att_sp.thrust;
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
yaw_sp = _att_sp.yaw_body;
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();
_wheel_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(_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();
_wheel_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.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();
_wheel_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* 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.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
/* allow manual control of rudder deflection */
if (!_vcontrol_mode.flag_control_velocity_enabled) {
yaw_manual = _manual.r;
throttle_sp = _manual.z;
}
/*
* in manual mode no external source should / does emit attitude setpoints.
* emit the manual setpoint here to allow attitude controller tuning
* in attitude control mode.
*/
struct vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
att_sp.roll_reset_integral = false;
att_sp.pitch_reset_integral = false;
att_sp.yaw_reset_integral = false;
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* If the aircraft is on ground reset the integrators */