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

FW Acro: udpate ECL and implemented fixed-wing Acro

Browse Source
This commit is contained in:
Andreas Antener 2017-02-11 10:58:36 +01:00 committed by Andreas Daniel Antener
parent 2f3b1edbd4
commit b19cd19411
3 changed files with 206 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

@ -1 +1 @@
Subproject commit bcf7cac5d95e509ac5eccb26ccbe6c890df8c17c
Subproject commit 901796ff8af2d147937d9e234cc197e85415c01f

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

@ -171,9 +171,11 @@ private:
float r_rmax;
float y_p;
float y_i;
float y_d;
float y_ff;
float y_integrator_max;
float y_coordinated_min_speed;
float roll_to_yaw_ff;
int32_t y_coordinated_method;
float y_rmax;
float w_p;
@ -199,6 +201,10 @@ private:
float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */
float man_yaw_scale; /**< scale factor applied to yaw actuator control in pure manual mode */
float acro_max_x_rate_rad;
float acro_max_y_rate_rad;
float acro_max_z_rate_rad;
float flaps_scale; /**< Scale factor for flaps */
float flaperon_scale; /**< Scale factor for flaperons */
@ -225,9 +231,11 @@ private:
param_t r_rmax;
param_t y_p;
param_t y_i;
param_t y_d;
param_t y_ff;
param_t y_integrator_max;
param_t y_coordinated_min_speed;
param_t roll_to_yaw_ff;
param_t y_coordinated_method;
param_t y_rmax;
param_t w_p;
@ -251,6 +259,10 @@ private:
param_t man_pitch_scale;
param_t man_yaw_scale;
param_t acro_max_x_rate;
param_t acro_max_y_rate;
param_t acro_max_z_rate;
param_t flaps_scale;
param_t flaperon_scale;
@ -420,6 +432,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_ff = param_find("FW_YR_FF");
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
_parameter_handles.roll_to_yaw_ff = param_find("FW_RLL_TO_YAW_FF");
_parameter_handles.w_p = param_find("FW_WR_P");
_parameter_handles.w_i = param_find("FW_WR_I");
@ -446,6 +459,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.man_pitch_scale = param_find("FW_MAN_P_SC");
_parameter_handles.man_yaw_scale = param_find("FW_MAN_Y_SC");
_parameter_handles.acro_max_x_rate = param_find("FW_ACRO_X_MAX");
_parameter_handles.acro_max_y_rate = param_find("FW_ACRO_Y_MAX");
_parameter_handles.acro_max_z_rate = param_find("FW_ACRO_Z_MAX");
_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
@ -513,6 +530,7 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
@ -539,6 +557,13 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
param_get(_parameter_handles.acro_max_x_rate, &(_parameters.acro_max_x_rate_rad));
param_get(_parameter_handles.acro_max_y_rate, &(_parameters.acro_max_y_rate_rad));
param_get(_parameter_handles.acro_max_z_rate, &(_parameters.acro_max_z_rate_rad));
_parameters.acro_max_x_rate_rad = math::radians(_parameters.acro_max_x_rate_rad);
_parameters.acro_max_y_rate_rad = math::radians(_parameters.acro_max_y_rate_rad);
_parameters.acro_max_z_rate_rad = math::radians(_parameters.acro_max_z_rate_rad);
param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
@ -866,7 +891,7 @@ FixedwingAttitudeControl::task_main()
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
if (_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing) {
lock_integrator = false;
} else {
@ -931,12 +956,14 @@ FixedwingAttitudeControl::task_main()
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (_vcontrol_mode.flag_control_rates_enabled) {
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) {
bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed);
/* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */
if (nonfinite || !_ctrl_state.airspeed_valid) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
@ -1017,25 +1044,14 @@ FixedwingAttitudeControl::task_main()
_wheel_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
control_input.acc_body_x = _accel.x;
control_input.acc_body_y = _accel.y;
control_input.acc_body_z = _accel.z;
control_input.body_x_rate = _ctrl_state.roll_rate;
control_input.body_y_rate = _ctrl_state.pitch_rate;
control_input.body_z_rate = _ctrl_state.yaw_rate;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.yaw_setpoint = yaw_sp;
@ -1050,114 +1066,139 @@ FixedwingAttitudeControl::task_main()
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
/* Run attitude controllers */
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
}
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
} else {
// pure rate control
_roll_ctrl.set_bodyrate_setpoint(_manual.y * _parameters.acro_max_x_rate_rad);
_pitch_ctrl.set_bodyrate_setpoint(-_manual.x * _parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_bodyrate_setpoint(_manual.r * _parameters.acro_max_z_rate_rad);
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
} else {
yaw_u = _yaw_ctrl.control_bodyrate(control_input);
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through if it is finite and if no engine failure was detected */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
//!(_vehicle_status.engine_failure ||
!_vehicle_status.engine_failure_cmd) ?
throttle_sp : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
}
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
_rates_sp.roll = _roll_ctrl.get_desired_rate();
_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
_rates_sp.timestamp = hrt_absolute_time();
@ -1179,6 +1220,11 @@ FixedwingAttitudeControl::task_main()
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain(
_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;

54
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -272,6 +272,21 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
/**
* Roll control to yaw control feedforward gain.
*
* This gain can be used to counteract the "adverse yaw" effect for fixed wings.
* When the plane enters a roll it will tend to yaw the nose out of the turn.
* This gain enables the use of a yaw actuator (rudder, airbrakes, ...) to counteract
* this effect.
*
* @min 0.0
* @decimal 1
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
/**
* Wheel steering rate proportional gain
*
@ -575,3 +590,42 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
* @group FW Attitude Control
*/
PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
/**
* Acro body x max rate.
*
* This is the rate the controller is trying to achieve if the user applies full roll
* stick input in acro mode.
*
* @min 45
* @max 720
* @unit degrees
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
/**
* Acro body y max rate.
*
* This is the body y rate the controller is trying to achieve if the user applies full pitch
* stick input in acro mode.
*
* @min 45
* @max 720
* @unit degrees
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
/**
* Acro body z max rate.
*
* This is the body z rate the controller is trying to achieve if the user applies full yaw
* stick input in acro mode.
*
* @min 10
* @max 180
* @unit degrees
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);