diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index d75faf92faacd..b0cbe34087e29 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -751,7 +751,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) const float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); @@ -905,7 +905,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi // Calculate additional throttle for turn drag compensation including throttle nudging const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f);