diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 17cc78bc17397..d56db22d70abb 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -657,7 +657,7 @@ void Plane::update_load_factor(void) } #endif - float max_load_factor = smoothed_airspeed / MAX(aparm.airspeed_min, 1); + float max_load_factor = sq(smoothed_airspeed / MAX(aparm.airspeed_min, 1)); if (max_load_factor <= 1) { // our airspeed is below the minimum airspeed. Limit roll to // 25 degrees @@ -668,13 +668,13 @@ void Plane::update_load_factor(void) // load limit. Limit our roll to a bank angle that will keep // the load within what the airframe can handle. We always // allow at least 25 degrees of roll however, to ensure the - // aircraft can be manoeuvred with a bad airspeed estimate. At + // aircraft can be manoeuvered with a bad airspeed estimate. At // 25 degrees the load factor is 1.1 (10%) - int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100; + int32_t roll_limit = degrees(acosf(1.0f / max_load_factor))*100; if (roll_limit < 2500) { roll_limit = 2500; } nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); roll_limit_cd = MIN(roll_limit_cd, roll_limit); - } + } }