diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 |
3 files changed, 1 insertions, 12 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a0a18bc2e..46db788a6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index d2a231694..9894a34d7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; //xxx: param - /* input conditioning */ // warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 79184e2cd..fe03b8065 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = 0.0f; if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison + if(fabsf(denumerator) > FLT_EPSILON) { _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ |