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

fw_attitude_control: calculate attitude setpoint for STAB mode

Browse Source 
- attitude setpoint generation for stabilized mode was shifted back
to the fw attitude controller. since the fw position controller is polling
on global position attitude setpoints were not generated when global
position was not published.

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-07-20 13:21:18 +02:00 committed by Andreas Antener
parent f069880504
commit aea7bd5b47
2 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -975,6 +975,19 @@ FixedwingAttitudeControl::task_main()
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
// in STABILIZED mode we need to generate the attitude setpoint
// from manual user inputs
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
_att_sp.yaw_body = 0.0f;
_att_sp.thrust = _manual.z;
int instance;
orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
}
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
yaw_sp = _att_sp.yaw_body;

24
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1942,7 +1942,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
} else {
}
if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
@ -2004,23 +2007,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
if (_control_mode.flag_control_attitude_enabled) {
/** STABILIZED FLIGHT **/
_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
_att_sp.yaw_body = 0.0f;
_att_sp.thrust = _manual.z;
} else {
/** MANUAL FLIGHT **/
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.thrust = 0.0f;
/* do not publish the setpoint */
setpoint = false;
}
/* do not publish the setpoint */
setpoint = false;
// reset hold altitude
_hold_alt = _global_pos.alt;