diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp index c3c4a96388..d6290cfe2c 100644 --- a/attitude_fw/ecl_wheel_controller.cpp +++ b/attitude_fw/ecl_wheel_controller.cpp @@ -54,12 +54,12 @@ ECL_WheelController::ECL_WheelController() : float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.body_z_rate) && - PX4_ISFINITE(ctl_data.groundspeed) && - PX4_ISFINITE(ctl_data.groundspeed_scaler))) { - return math::constrain(_last_output, -1.0f, 1.0f); - } + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -76,8 +76,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { @@ -102,9 +102,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da //xxx: until start detection is available: integral part in control signal is limited here float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* Apply PI rate controller and store non-limited output */ - _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + - ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained); + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + integrator_constrained); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -118,10 +118,10 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da return _rate_setpoint; } - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - /* shortest angle (wrap around) */ - yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = yaw_error / _tc; diff --git a/attitude_fw/ecl_wheel_controller.h b/attitude_fw/ecl_wheel_controller.h index 166899bd8f..10a8f85656 100644 --- a/attitude_fw/ecl_wheel_controller.h +++ b/attitude_fw/ecl_wheel_controller.h @@ -61,11 +61,11 @@ public: ECL_WheelController(); ~ECL_WheelController() = default; - float control_attitude(const struct ECL_ControlData &ctl_data); + float control_attitude(const struct ECL_ControlData &ctl_data); - float control_bodyrate(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); - float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}; + float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}; }; #endif // ECL_HEADING_CONTROLLER_H diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp index 3472d57628..9feb80ab58 100644 --- a/attitude_fw/ecl_yaw_controller.cpp +++ b/attitude_fw/ecl_yaw_controller.cpp @@ -91,6 +91,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control float constrained_roll; bool inverted = false; + /* roll is used as feedforward term and inverted flight needs to be considered */ if (fabsf(ctl_data.roll) < math::radians(90.0f)) { /* not inverted, but numerically still potentially close to infinity */ @@ -98,8 +99,9 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control } else { inverted = true; - // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity - //note: the ranges are extended by 10 deg here to avoid numeric resolution effects + + // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity + //note: the ranges are extended by 10 deg here to avoid numeric resolution effects if (ctl_data.roll > 0.0f) { /* right hemisphere */ constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); @@ -115,10 +117,12 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control if (!inverted) { /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ? + ctl_data.airspeed_min : ctl_data.airspeed); } /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate > 0.01f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;