aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_att_control
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fw_att_control')
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp79
-rw-r--r--src/modules/fw_att_control/fw_att_control_params.c33
2 files changed, 58 insertions, 54 deletions
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 91981b08a..78952cb8d 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -156,6 +156,7 @@ private:
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated;
+ float y_rmax;
float airspeed_min;
float airspeed_trim;
@@ -183,6 +184,7 @@ private:
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;
@@ -286,25 +288,26 @@ 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_IMAX");
+ _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_IMAX");
+ _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_IMAX");
+ _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");
@@ -366,6 +369,7 @@ FixedwingAttitudeControl::parameters_update()
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));
@@ -373,29 +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;
}
@@ -437,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;
}
@@ -596,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;
@@ -670,9 +673,9 @@ FixedwingAttitudeControl::task_main()
float speed_body_u = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
- speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[0][1] * _global_pos.vy + _att.R[0][2] * _global_pos.vz;
-// speed_body_v = _att.R[1][0] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[1][2] * _global_pos.vz;
- speed_body_w = _att.R[2][0] * _global_pos.vx + _att.R[2][1] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
+ speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
+// speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
+ speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
} else {
warnx("Did not get a valid R\n");
}
@@ -685,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(pitch_u)) ? pitch_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;
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 39b78e20a..41e37e61f 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, 0.2f);
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @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, 0.2f);
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @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,12 +128,13 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 0.2f);
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, 0.2f);
-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, 0.2f);
+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-10 23:17:43 +0000