fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 09:34:08 +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-06-27 10:53:39 +02:00 committed by Andreas Antener
parent e3f3233ee4
commit fd51bf44d5
2 changed files with 31 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
src/modules/fw_att_control/fw_att_control_main.cpp
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 */

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

@ -290,6 +290,7 @@ private:
float throttle_cruise;
float throttle_slew_max;
float man_roll_max_rad;
float man_pitch_max_rad;
float rollsp_offset_rad;
float pitchsp_offset_rad;
@ -345,6 +346,7 @@ private:
param_t throttle_cruise;
param_t throttle_slew_max;
param_t man_roll_max_deg;
param_t man_pitch_max_deg;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
@ -624,6 +626,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max_deg = param_find("FW_MAN_P_MAX");
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
@ -710,6 +713,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
param_get(_parameter_handles.man_pitch_max_deg, &_parameters.man_pitch_max_rad);
_parameters.man_pitch_max_rad = math::radians(_parameters.man_pitch_max_rad);
param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
@ -1996,6 +2001,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_control_mode_current = FW_POSCTRL_MODE_OTHER;
/** MANUAL FLIGHT **/
setpoint = false;
if (_control_mode.flag_control_attitude_enabled) {
_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;
setpoint = true;
}
// reset hold altitude
_hold_alt = _global_pos.alt;
@ -2007,9 +2020,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// reset lading abort state
_fw_pos_ctrl_status.abort_landing = false;
/* no flight mode applies, do not publish an attitude setpoint */
setpoint = false;
/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();
@ -2040,7 +2050,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
_att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max);
} else {
/* Copy thrust and pitch values from tecs */
if (_vehicle_land_detected.landed) {