fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 13:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make throttle time constant tunable separately, group TECS params correctly, make climbout alt configurable

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-01 13:10:07 +02:00
parent 8e33278c4c
commit 3b1c92e42f
2 changed files with 46 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+10 -1
View File
@@ -203,9 +203,11 @@ private:
float l1_damping;
float time_const;
float time_const_throt;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float climbout_diff;
float heightrate_p;
float speedrate_p;
float throttle_damp;
@@ -245,9 +247,11 @@ private:
param_t l1_damping;
param_t time_const;
param_t time_const_throt;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t climbout_diff;
param_t heightrate_p;
param_t speedrate_p;
param_t throttle_damp;
@@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
@@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
@@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
@@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_time_const_throt(_parameters.time_const_throt);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
@@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+36 -13
View File
@@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Climbout Altitude difference
*
* If the altitude error exceeds this parameter, the system will climb out
* with maximum throttle and minimum airspeed until it is closer than this
* distance to the desired altitude. Mostly used for takeoff waypoints / modes.
* Set to zero to disable climbout mode (not recommended).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
/**
* Maximum climb rate
*
@@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
@@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
@@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* TECS Throttle time constant
*
* This is the time constant of the TECS throttle control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
@@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
@@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
@@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
@@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
@@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
@@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
@@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);