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

renamed heading controller to wheel controller, added groundspeed dependency and separate parameters

Browse Source
This commit is contained in:
Andreas Antener
2015-11-17 22:28:08 +01:00
committed by Roman
parent 6c31421889
commit 234a200e60
9 changed files with 138 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+40 -11
View File
@@ -82,7 +82,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <ecl/attitude_fw/ecl_heading_controller.h>
#include <ecl/attitude_fw/ecl_wheel_controller.h>
#include <platforms/px4_defines.h>
/**
@@ -182,6 +182,11 @@ private:
float y_coordinated_min_speed;
int32_t y_coordinated_method;
float y_rmax;
float w_p;
float w_i;
float w_ff;
float w_integrator_max;
float w_rmax;
float airspeed_min;
float airspeed_trim;
@@ -223,6 +228,11 @@ private:
param_t y_coordinated_min_speed;
param_t y_coordinated_method;
param_t y_rmax;
param_t w_p;
param_t w_i;
param_t w_ff;
param_t w_integrator_max;
param_t w_rmax;
param_t airspeed_min;
param_t airspeed_trim;
@@ -249,7 +259,7 @@ private:
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_HeadingController _heading_ctrl;
ECL_WheelController _wheel_ctrl;
/**
@@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
_parameter_handles.w_p = param_find("FW_WR_P");
_parameter_handles.w_i = param_find("FW_WR_I");
_parameter_handles.w_ff = param_find("FW_WR_FF");
_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
_parameter_handles.w_rmax = param_find("FW_W_RMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
@@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
@@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update()
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* heading control parameters */
_heading_ctrl.set_k_p(_parameters.y_p);
_heading_ctrl.set_k_i(_parameters.y_i);
_heading_ctrl.set_k_ff(_parameters.y_ff);
_heading_ctrl.set_integrator_max(_parameters.y_integrator_max);
_heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* wheel control parameters */
_wheel_ctrl.set_k_p(_parameters.w_p);
_wheel_ctrl.set_k_i(_parameters.w_i);
_wheel_ctrl.set_k_ff(_parameters.w_ff);
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
return OK;
}
@@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
@@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
@@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else {
/*
@@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
@@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main()
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e);
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
@@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main()
_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
_heading_ctrl.control_attitude(control_input);
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main()
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _heading_ctrl.control_bodyrate(control_input);
if (_att_sp.fw_control_wheel == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
}
else {
@@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[2] += 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);
src/modules/fw_att_control/fw_att_control_params.c
+60
View File
@@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
/**
* Wheel steering rate proportional gain
*
* This defines how much the wheel steering input will be commanded depending on the
* current body angular rate error.
*
* @min 0.005
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f);
/**
* Wheel steering rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f);
/**
* Wheel steering rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f);
/**
* Maximum wheel steering rate
*
* This limits the maximum wheel steering rate the controller will output (in degrees per
* second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
/**
* Roll rate feed forward
*
@@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
/**
* Wheel steering rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
/**
* Minimal speed for yaw coordination
*
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+4 -3
View File
@@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool setpoint = true;
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// assign values
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
_att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly
_att_sp.fw_control_wheel = _runway_takeoff.controlWheel();
_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
} else {
/* Perform launch detection */