dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

reintroduce feedforward

This commit is contained in:
Thomas Gubler 2013-11-13 11:05:22 +01:00
parent 8e92d47de1
commit 38172497c1
8 changed files with 59 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -53,6 +53,7 @@ ECL_PitchController::ECL_PitchController() :
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
_max_rate_neg(0.0f),
@ -173,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);

10
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@ -73,21 +73,30 @@ public:
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate_pos(float max_rate_pos) {
_max_rate_pos = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
_max_rate_neg = max_rate_neg;
}
void set_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
@ -111,6 +120,7 @@ private:
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate_pos;
float _max_rate_neg;

3
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -53,6 +53,7 @@ ECL_RollController::ECL_RollController() :
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_last_output(0.0f),
@ -141,7 +142,7 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}

10
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@ -71,18 +71,27 @@ public:
_tc = time_constant;
}
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
@ -105,6 +114,7 @@ private:
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
float _last_output;

3
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() :
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_roll_ff(0.0f),
@ -159,7 +160,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);

13
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@ -69,22 +69,32 @@ public:
void set_k_p(float k_p) {
_k_p = k_p;
}
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
void set_k_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
@ -107,6 +117,7 @@ private:
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
float _roll_ff;

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

@ -141,6 +141,7 @@ private:
float p_p;
float p_d;
float p_i;
float p_ff;
float p_rmax_pos;
float p_rmax_neg;
float p_integrator_max;
@ -148,11 +149,13 @@ private:
float r_p;
float r_d;
float r_i;
float r_ff;
float r_integrator_max;
float r_rmax;
float y_p;
float y_i;
float y_d;
float y_ff;
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated_min_speed;
@ -169,6 +172,7 @@ private:
param_t p_p;
param_t p_d;
param_t p_i;
param_t p_ff;
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
@ -176,11 +180,13 @@ private:
param_t r_p;
param_t r_d;
param_t r_i;
param_t r_ff;
param_t r_integrator_max;
param_t r_rmax;
param_t y_p;
param_t y_i;
param_t y_d;
param_t y_ff;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t y_coordinated_min_speed;
@ -302,6 +308,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_d = param_find("FW_PR_D");
_parameter_handles.p_i = param_find("FW_PR_I");
_parameter_handles.p_ff = param_find("FW_PR_FF");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
@ -310,12 +317,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_d = param_find("FW_RR_D");
_parameter_handles.r_i = param_find("FW_RR_I");
_parameter_handles.r_ff = param_find("FW_RR_FF");
_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_YR_P");
_parameter_handles.y_i = param_find("FW_YR_I");
_parameter_handles.y_d = param_find("FW_YR_D");
_parameter_handles.y_ff = param_find("FW_YR_FF");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
@ -363,6 +372,7 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_d, &(_parameters.p_d));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
@ -371,12 +381,15 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_d, &(_parameters.r_d));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_d, &(_parameters.y_d));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
@ -391,6 +404,7 @@ FixedwingAttitudeControl::parameters_update()
_pitch_ctrl.set_k_p(_parameters.p_p);
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_d(_parameters.p_d);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
@ -401,6 +415,7 @@ FixedwingAttitudeControl::parameters_update()
_roll_ctrl.set_k_p(_parameters.r_p);
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_d(_parameters.r_d);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
@ -408,6 +423,7 @@ FixedwingAttitudeControl::parameters_update()
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_d(_parameters.y_d);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@ -138,3 +138,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f);