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

wip fw ctrl, several small bugfixes, set limit to 1

Browse Source
This commit is contained in:
Thomas Gubler
2013-10-24 18:08:23 +02:00
parent 3c14483df2
commit 146279b20f
8 changed files with 44 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+5 -6
View File
@@ -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)
{
}
@@ -148,10 +147,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
/*
* 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);
}
@@ -163,9 +162,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
_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^2 is proportional to 1/airspeed^2
_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()
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
-1
View File
@@ -120,7 +120,6 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_PITCH_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+8 -7
View File
@@ -54,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)
{
}
@@ -96,6 +95,7 @@ float ECL_RollController::control_bodyrate(float pitch,
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);
@@ -120,10 +120,10 @@ float ECL_RollController::control_bodyrate(float pitch,
/*
* 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);
}
@@ -133,12 +133,13 @@ float ECL_RollController::control_bodyrate(float pitch,
/* integrator limit */
_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_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^2 is proportional to 1/airspeed^2
_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()
src/lib/ecl/attitude_fw/ecl_roll_controller.h
-1
View File
@@ -112,7 +112,6 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_ROLL_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+4 -5
View File
@@ -54,7 +54,6 @@ 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)
{
@@ -135,10 +134,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
/*
* 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);
}
@@ -150,9 +149,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
_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^2 is proportional to 1/airspeed^2
_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()
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
-1
View File
@@ -117,7 +117,6 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _max_deflection_rad;
float _coordinated;
};
src/modules/fw_att_control/fw_att_control_main.cpp
+26 -23
View File
@@ -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;
@@ -305,6 +307,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_Y_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;
@@ -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(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;
src/modules/fw_att_control/fw_att_control_params.c
+1
View File
@@ -136,4 +136,5 @@ 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);