From b60964eb9c1e24b14c0cbf4527bf6b7e4bb5fd40 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 13:27:53 +0200 Subject: Multirotor mixer: more careful limiting --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 70 +++++++++++------------- 1 file changed, 33 insertions(+), 37 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index bf77795d5..740a22781 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -282,56 +282,52 @@ MultirotorMixer::mix(float *outputs, unsigned space) float yaw = get_control(0, 2) * _yaw_scale; float thrust = get_control(0, 3); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); - float max = 0.0f; + float min_out = 0.0f; + float max_out = 0.0f; float fixup_scale; - /* use an output factor to prevent too strong control signals at low throttle */ - float min_thrust = 0.05f; - float max_thrust = 1.0f; - float startpoint_full_control = 0.40f; - float output_factor; - - /* keep roll, pitch and yaw control to 0 below min thrust */ - if (thrust <= min_thrust) { - output_factor = 0.0f; - /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ - - } else if (thrust < startpoint_full_control && thrust > min_thrust) { - output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); - /* and then stay at full control */ - - } else { - output_factor = max_thrust; - } - - roll *= output_factor; - pitch *= output_factor; - yaw *= output_factor; - - - /* perform initial mix pass yielding un-bounded outputs */ + /* perform initial mix pass yielding unbounded outputs */ for (unsigned i = 0; i < _rotor_count; i++) { - float tmp = roll * _rotors[i].roll_scale + + float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale + + yaw * _rotors[i].yaw_scale + thrust; - if (tmp > max) - max = tmp; + if (out < min_out) { + min_out = out; + } + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; + } + + /* scale down controls if some outputs are negative, keep total thrust */ + if (min_out < 0.0) { + float scale_in = thrust / (thrust - min_out); - outputs[i] = tmp; + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = scale_in * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale) + + thrust; + } } - /* scale values into the -1.0 - 1.0 range */ - if (max > 1.0f) { - fixup_scale = 2.0f / max; + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; } else { - fixup_scale = 2.0f; + scale_out = 1.0f; } - for (unsigned i = 0; i < _rotor_count; i++) - outputs[i] = -1.0f + (outputs[i] * fixup_scale); + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = -1.0f + (outputs[i] * 2 * scale_out); + } /* ensure outputs are out of the deadband */ for (unsigned i = 0; i < _rotor_count; i++) -- cgit v1.2.3 From ae1faa6de6d6952af73a8a9367625fbf96822fe1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 13:45:43 +0200 Subject: MC mixer input limiting implemented. --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 740a22781..2af9d913d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -67,6 +67,11 @@ namespace { +float constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + /* * These tables automatically generated by multi_tables - do not edit. */ @@ -276,11 +281,11 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl unsigned MultirotorMixer::mix(float *outputs, unsigned space) { - float roll = get_control(0, 0) * _roll_scale; + float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); - float pitch = get_control(0, 1) * _pitch_scale; - float yaw = get_control(0, 2) * _yaw_scale; - float thrust = get_control(0, 3); + float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); + float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); + float thrust = constrain(get_control(0, 3), 0.0f, 1.0f); //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; -- cgit v1.2.3 From b9b84b08b79dd6661905cfd5d4fa8578ea392bec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 14:01:55 +0200 Subject: Multirotor mixer: limit yaw first, then roll/pitch --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2af9d913d..672784f46 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -289,15 +289,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float fixup_scale; + float scale_yaw = 1.0f; - /* perform initial mix pass yielding unbounded outputs */ + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale + thrust; + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ if (out < min_out) { min_out = out; } @@ -308,16 +313,19 @@ MultirotorMixer::mix(float *outputs, unsigned space) outputs[i] = out; } - /* scale down controls if some outputs are negative, keep total thrust */ + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ if (min_out < 0.0) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = scale_in * (roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - yaw * _rotors[i].yaw_scale) + - thrust; + outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += yaw * _rotors[i].yaw_scale; } } -- cgit v1.2.3 From bc3ca8db5646cf2a2e235cf7ca3bd62335e062c2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 14:26:32 +0200 Subject: Multirotor mixer: yaw limiting bug fixed --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 672784f46..8568f9e60 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -299,7 +299,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { - yaw = out / _rotors[i].yaw_scale; + yaw = -out / _rotors[i].yaw_scale; } /* calculate min and max output values */ -- cgit v1.2.3 From d9a7e528b056556112c74d13a86f30bdab88f635 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 15:44:56 +0200 Subject: Multirotor mixer: idle_speed (aka deadband) fixed --- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 12 ++++-------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1c889a811..fa9d28b74 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -516,7 +516,7 @@ private: float _roll_scale; float _pitch_scale; float _yaw_scale; - float _deadband; + float _idle_speed; unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8568f9e60..1ca0a21e9 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -176,12 +176,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, float roll_scale, float pitch_scale, float yaw_scale, - float deadband) : + float idle_speed) : Mixer(control_cb, cb_handle), _roll_scale(roll_scale), _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), - _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _rotor_count(_config_rotor_count[geometry]), _rotors(_config_index[geometry]) { @@ -338,15 +338,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) scale_out = 1.0f; } + /* scale outputs to range _idle_speed..1 */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = -1.0f + (outputs[i] * 2 * scale_out); + outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out); } - /* ensure outputs are out of the deadband */ - for (unsigned i = 0; i < _rotor_count; i++) - if (outputs[i] < _deadband) - outputs[i] = _deadband; - return _rotor_count; } -- cgit v1.2.3 From e72a7a7dd78cde4a93d42a53328553e71560fc27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:14 +0200 Subject: fw att: robustify main loop against non finite numbers and limit error output rate --- src/modules/fw_att_control/fw_att_control_main.cpp | 42 ++++++++++++++++++---- 1 file changed, 35 insertions(+), 7 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 1f3e9f098..cafce4417 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main() while (!_task_should_exit) { + static int loop_counter = 0; + /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); @@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - warnx("Did not get a valid R\n"); + if (loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } } /* Run attitude controllers */ @@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { - warnx("roll_u %.4f", roll_u); + _roll_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("roll_u %.4f", roll_u); + } } float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { - warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); + _pitch_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); + } } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", (double)yaw_u); + _yaw_ctrl.reset_integrator(); + if (loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double)yaw_u); + } } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", (double)throttle_sp); + if (loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double)throttle_sp); + } } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + if (loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); + } } /* @@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main() } + loop_counter++; perf_end(_loop_perf); } -- cgit v1.2.3 From 7c165689ce0d33a65554f62a3438c2da30404642 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:37 +0200 Subject: fw att: pitch ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 9584924cc..a3f5199b1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() : float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { - + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + warnx("not controlling pitch"); + return _rate_setpoint; + } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -123,6 +127,13 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); -- cgit v1.2.3 From 52596be98c77497d67615435fdbd8e403cae618f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:15:50 +0200 Subject: fw att: roll ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 2e86c72dc..6ad00049d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() : float ECL_RollController::control_attitude(float roll_setpoint, float roll) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll_setpoint) && isfinite(roll))) { + return _rate_setpoint; + } /* Calculate error */ float roll_error = roll_setpoint - roll; @@ -86,6 +90,13 @@ float ECL_RollController::control_bodyrate(float pitch, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && + isfinite(airspeed_min) && isfinite(airspeed_max) && + isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); @@ -122,8 +133,8 @@ float ECL_RollController::control_bodyrate(float pitch, float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ + * anti-windup: do not allow integrator to increase if actuator is at limit + */ if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); -- cgit v1.2.3 From 9a35bac2adc1e803dea7cdb6a2db277f111724e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 May 2014 00:16:10 +0200 Subject: fw att: yaw ctrl: robustify against non finite numbers --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 255776765..d43e0314e 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,6 +66,12 @@ float ECL_YawController::control_attitude(float roll, float pitch, float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && + isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && + isfinite(pitch_rate_setpoint))) { + return _rate_setpoint; + } // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; @@ -103,6 +109,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float pitch_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && + isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && + isfinite(airspeed_max) && isfinite(scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); -- cgit v1.2.3