From 3042731d26e94b3c126e61e422b98fcd7df4694c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:27:26 +0100 Subject: [PATCH 1/4] Smaller hotfixes for att pos estimator --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 33879892ee..18fb6dcbc9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -34,7 +34,7 @@ /** * @file KalmanNav.cpp * - * kalman filter navigation code + * Kalman filter navigation code */ #include @@ -228,10 +228,7 @@ void KalmanNav::update() updateSubscriptions(); // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate && - _sensors.accelerometer_counter > 10 && - _sensors.gyro_counter > 10 && - _sensors.magnetometer_counter > 10) { + if (!_attitudeInitialized && sensorsUpdate) { if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { @@ -643,7 +640,7 @@ int KalmanNav::correctAtt() if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:\n"); y.print(); + warnx("y:"); y.print(); } // update quaternions from euler From ed60dc50fcf2ab4dde76e937244bfb6eae81c709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 07:43:08 +0100 Subject: [PATCH 2/4] Hotfix: forbid integrator to accumulate NaN values if they ever would occur --- .../fw_att_control/fw_att_control_main.cpp | 61 ++++++++++--------- 1 file changed, 32 insertions(+), 29 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 eb67874db4..b3973084f3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + if (isfinite(roll_sp) && isfinite(pitch_sp)) { - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); - rates_sp.timestamp = hrt_absolute_time(); + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } } } else { From 1358d4cb88e70370014410353faf3e51afb1ceb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 07:44:16 +0100 Subject: [PATCH 3/4] Hotfix: Fix integrator parameters --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 b3973084f3..00a0dcd619 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_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_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_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_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_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_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); From d3b267c06e53aaf119b66717ac33e78834ea0d69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Nov 2013 09:20:07 +0100 Subject: [PATCH 4/4] Integral fixes, last parts --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 ++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39b..2eb58abd66 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - - float dt = dt_micros / 1000000; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) @@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc _rate_error = _rate_setpoint - pitch_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc029..0b1ffa7a4d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_error = _rate_setpoint - roll_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {