From b16e6249e4189e077ce9d82973897a7698842181 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 11 Oct 2015 16:52:09 +0200 Subject: [PATCH] more correct groundspeed scaling for wheel controller --- src/lib/ecl/attitude_fw/ecl_controller.h | 3 ++- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 19 +++++++------------ .../fw_att_control/fw_att_control_main.cpp | 13 ++++++++++--- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 0817204ccc..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,7 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; - float ground_speed; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index d67a46bb1c..76419ef6f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -60,7 +60,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.ground_speed))) { + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -79,21 +80,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* assume minimum speed to prevent oscillations */ - float speed = min_speed; - if (ctl_data.ground_speed > speed) { - speed = ctl_data.ground_speed; - } - - /* only scale a certain amount with speed else the corrections get to small */ - float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.groundspeed_scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -115,7 +108,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ 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 da91fda9f7..271dc86079 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -837,9 +837,16 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_sp = 0.0f; @@ -995,8 +1002,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);