aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp32
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.h1
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp39
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.h1
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp53
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.h5
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp85
-rw-r--r--src/modules/fw_att_control/fw_att_control_params.c34
8 files changed, 139 insertions, 111 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index e1f4d3eef..531512493 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -51,8 +51,7 @@ ECL_PitchController::ECL_PitchController() :
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
- _rate_setpoint(0.0f),
- _max_deflection_rad(math::radians(45.0f))
+ _rate_setpoint(0.0f)
{
}
@@ -116,15 +115,14 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
-
- float dt = dt_micros / 1000000;
+ float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
- float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
- float k_i_rate = _k_i * _tc;
+// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+ float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
@@ -142,20 +140,17 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
- float ilimit_scaled = 0.0f;
-
- if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * k_i_rate * dt * scaler;
+ float id = _rate_error * dt;
/*
- * anti-windup: do not allow integrator to increase into the
- * wrong direction if actuator is at limit
+ * anti-windup: do not allow integrator to increase if actuator is at limit
*/
- if (_last_output < -_max_deflection_rad) {
+ if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
- } else if (_last_output > _max_deflection_rad) {
+ } else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -164,11 +159,12 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
}
/* integrator limit */
- _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
- /* store non-limited output */
- _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
+ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
- return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index ef6129d02..ba8ed3e6f 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -120,7 +120,6 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
- float _max_deflection_rad;
};
#endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 3afda3176..e5bd7b4f7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,6 +45,7 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
@@ -53,8 +54,7 @@ ECL_RollController::ECL_RollController() :
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
- _bodyrate_setpoint(0.0f),
- _max_deflection_rad(math::radians(45.0f))
+ _bodyrate_setpoint(0.0f)
{
}
@@ -85,12 +85,17 @@ float ECL_RollController::control_bodyrate(float pitch,
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
- float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+ float dt = (float)dt_micros * 1e-6f;
- float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
- float k_i_rate = _k_i * _tc;
+ /* lock integral for long intervals */
+ if (dt_micros > 500000)
+ lock_integrator = true;
+
+// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+ float k_ff = 0; //xxx: param
/* input conditioning */
+// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -108,20 +113,17 @@ float ECL_RollController::control_bodyrate(float pitch,
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
- float ilimit_scaled = 0.0f;
-
- if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * k_i_rate * dt * scaler;
+ float id = _rate_error * dt;
/*
- * anti-windup: do not allow integrator to increase into the
- * wrong direction if actuator is at limit
+ * anti-windup: do not allow integrator to increase if actuator is at limit
*/
- if (_last_output < -_max_deflection_rad) {
+ if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
- } else if (_last_output > _max_deflection_rad) {
+ } else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -130,11 +132,14 @@ float ECL_RollController::control_bodyrate(float pitch,
}
/* integrator limit */
- _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
- /* store non-limited output */
- _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+ warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
+ warnx("roll: _last_output %.4f", (double)_last_output);
- return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index f94db0dc7..dd7d1bf53 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -112,7 +112,6 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
- float _max_deflection_rad;
};
#endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index d2cd9cf04..bc036e082 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,6 +44,7 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
@@ -53,7 +54,7 @@ ECL_YawController::ECL_YawController() :
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
- _max_deflection_rad(math::radians(45.0f))
+ _coordinated(1.0f)
{
@@ -63,11 +64,18 @@ float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
+// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
- float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
- if(denumerator != 0.0f) { //XXX: floating point comparison
- _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+ if (_coordinated > 0.1) {
+ float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+ if(denumerator != 0.0f) { //XXX: floating point comparison
+ _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+ }
+
+// if(counter % 20 == 0) {
+// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+// }
}
/* limit the rate */ //XXX: move to body angluar rates
@@ -76,6 +84,9 @@ float ECL_YawController::control_attitude(float roll, float pitch,
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
+
+// counter++;
+
return _rate_setpoint;
}
@@ -87,10 +98,16 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
- float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+ float dt = (float)dt_micros * 1e-6f;
+
+ /* lock integral for long intervals */
+ if (dt_micros > 500000)
+ lock_integrator = true;
+
+
+// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+ float k_ff = 0;
- float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
- float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
@@ -110,20 +127,17 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
- float ilimit_scaled = 0.0f;
-
- if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * k_i_rate * dt * scaler;
+ float id = _rate_error * dt;
/*
- * anti-windup: do not allow integrator to increase into the
- * wrong direction if actuator is at limit
+ * anti-windup: do not allow integrator to increase if actuator is at limit
*/
- if (_last_output < -_max_deflection_rad) {
+ if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
- } else if (_last_output > _max_deflection_rad) {
+ } else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -132,11 +146,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
}
/* integrator limit */
- _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
- /* store non-limited output */
- _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
- return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index d5aaa617f..5c00fa873 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -85,6 +85,9 @@ public:
void set_k_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
+ void set_coordinated(float coordinated) {
+ _coordinated = coordinated;
+ }
float get_rate_error() {
@@ -114,7 +117,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
- float _max_deflection_rad;
+ float _coordinated;
};
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index b59a97136..13c68f077 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -155,6 +155,8 @@ private:
float y_d;
float y_roll_feedforward;
float y_integrator_max;
+ float y_coordinated;
+ float y_rmax;
float airspeed_min;
float airspeed_trim;
@@ -181,6 +183,8 @@ private:
param_t y_d;
param_t y_roll_feedforward;
param_t y_integrator_max;
+ param_t y_coordinated;
+ param_t y_rmax;
param_t airspeed_min;
param_t airspeed_trim;
@@ -284,30 +288,33 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_airspeed_valid(false)
{
_parameter_handles.tconst = param_find("FW_ATT_TC");
- _parameter_handles.p_p = param_find("FW_P_P");
- _parameter_handles.p_d = param_find("FW_P_D");
- _parameter_handles.p_i = param_find("FW_P_I");
+ _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_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_P_integrator_max");
+ _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
- _parameter_handles.r_p = param_find("FW_R_P");
- _parameter_handles.r_d = param_find("FW_R_D");
- _parameter_handles.r_i = param_find("FW_R_I");
- _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
+ _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_integrator_max = param_find("FW_RR_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
- _parameter_handles.y_p = param_find("FW_Y_P");
- _parameter_handles.y_i = param_find("FW_Y_I");
- _parameter_handles.y_d = param_find("FW_Y_D");
+ _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_roll_feedforward = param_find("FW_Y_ROLLFF");
- _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
+ _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
+ _parameter_handles.y_rmax = param_find("FW_Y_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");
+ _parameter_handles.y_coordinated = param_find("FW_Y_COORD");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -361,6 +368,8 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_d, &(_parameters.y_d));
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, &(_parameters.y_coordinated));
+ param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -368,28 +377,30 @@ FixedwingAttitudeControl::parameters_update()
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
- _pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
- _pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
- _pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
- _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+ _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_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));
- _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+ _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
- _roll_ctrl.set_k_p(math::radians(_parameters.r_p));
- _roll_ctrl.set_k_i(math::radians(_parameters.r_i));
- _roll_ctrl.set_k_d(math::radians(_parameters.r_d));
- _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+ _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_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
- _yaw_ctrl.set_k_p(math::radians(_parameters.y_p));
- _yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
- _yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
- _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
- _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+ _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_roll_ff(_parameters.y_roll_feedforward);
+ _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
+ _yaw_ctrl.set_coordinated(_parameters.y_coordinated);
+ _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
return OK;
}
@@ -431,6 +442,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+ warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
return true;
}
@@ -590,9 +602,6 @@ FixedwingAttitudeControl::task_main()
if (_vcontrol_mode.flag_control_attitude_enabled) {
- /* scale from radians to normalized -1 .. 1 range */
- const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
-
/* scale around tuning airspeed */
float airspeed;
@@ -679,23 +688,23 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
/* Run attitude RATE controllers which need the desired attitudes from above */
- float roll_rad = _roll_ctrl.control_bodyrate(_att.pitch,
+ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
- _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+ _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
- float pitch_rad = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+ float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
- _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+ _actuators.control[1] = (isfinite(roll_u)) ? roll_u : 0.0f;
- float yaw_rad = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch,
+ float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
- _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+ _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
@@ -709,9 +718,9 @@ FixedwingAttitudeControl::task_main()
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
- 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.roll = _roll_ctrl.get_desired_rate();
+ rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+ rates_sp.yaw = _yaw_ctrl.get_desired_rate();
rates_sp.timestamp = hrt_absolute_time();
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 717608159..07fac4989 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -44,13 +44,13 @@
#include <systemlib/param/param.h>
-//XXX resolve unclear naming of paramters: FW_P_D --> FW_PR_P
+//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P
/*
* Controller parameters, accessible via MAVLink
*
*/
-
+//xxx: update descriptions
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Range 0.4 to 1.0 seconds, in tens of seconds
@@ -59,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Proportional gain.
// @Description This defines how much the elevator input will be commanded dependend on the current pitch error.
// @Range 10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
// @DisplayName Damping gain.
// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings.
// @Range 0.0 to 10.0, 0.1 increments
-PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove
// @DisplayName Integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
-PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
// @DisplayName Maximum positive / up pitch rate.
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
// @DisplayName Maximum negative / down pitch rate.
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
// @DisplayName Pitch Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 1000.0f);
// @DisplayName Roll feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
@@ -99,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
// @Range 10.0 200.0
// @Increment 10.0
// @User User
-PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f);
// @DisplayName Damping Gain
// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
// @Range 0.0 10.0
// @Increment 1.0
// @User User
-PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove
// @DisplayName Integrator Gain
// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
// @Range 0.0 100.0
// @Increment 5.0
// @User User
-PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
// @DisplayName Roll Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 1000.0f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -128,11 +128,13 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
-PARAM_DEFINE_FLOAT(FW_Y_P, 0);
-PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
-PARAM_DEFINE_FLOAT(FW_Y_D, 0);
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 1000.0f);
+PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
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_Y_COORD, 1.0f);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 00:04:54 +0000