diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-20 15:03:11 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-10-24 18:08:23 +0200 |
commit | 146279b20fc65705e31e03298a1d3eea8911d0f8 (patch) | |
tree | 846eaf2859548eabc100a08393f9bad6e9480de5 /src | |
parent | 3c14483df247fb842cabdbb02206912f7f4350cf (diff) | |
download | px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.tar.gz px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.tar.bz2 px4-firmware-146279b20fc65705e31e03298a1d3eea8911d0f8.zip |
wip fw ctrl, several small bugfixes, set limit to 1
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 1 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 9 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 49 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_params.c | 1 |
8 files changed, 44 insertions, 44 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 8b9ba9c62..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) { } @@ -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() 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 f3909593a..e5bd7b4f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -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() 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 84d9ebd88..bc036e082 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -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() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 0250fcb35..5c00fa873 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -117,7 +117,6 @@ 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 91981b08a..1b30462a8 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; @@ -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; 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 d61d3dd4f..c48f3a5e3 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -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); |