AP_TECS: remove unneccessary sqrtf in Phi calculations

[peterbarker]

we take the square of this in the only use of it

... note that the code doesn't match up with the description which says it is proportional to 1/cos(bank_angle) (not what the code is, which is 1/sq(cos(bank_angle)))

Tested in SITL using these patches:

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index 8a8f96cc72c..4382dc4f4d2 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -753,8 +753,13 @@ void AP_TECS::_update_throttle_with_airspeed(void)
         // 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 co
mpensate for induced
         // drag increase during turns.
+        const float old_cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*ro
tMat.b.y));
+        float old_STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(o
ld_cosPhi*old_cosPhi, 0.1f, 1.0f) - 1.0f);
+
         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);
+    ::fprintf(stderr, "%f xvs %f\n", old_STEdot_dem, STEdot_dem);
+
         const float ff_throttle = nomThr + STEdot_dem / K_thr2STE;
 
         // Calculate PD + FF throttle
@@ -907,8 +912,14 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi
     // 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
     // drag increase during turns.
+    float old_cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
+    float old_STEdot_dem = _rollComp * (1.0f/constrain_float(old_cosPhi * old_cosPhi, 0.1f, 1.0f) - 1.0f);
+
     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);
+
+    ::fprintf(stderr, "%f vs %f\n", old_STEdot_dem, STEdot_dem);
+
     _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
 
     constrain_throttle();
@@ -1501,4 +1512,4 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
 
     // don't allow max pitch to go below min pitch
     _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);

[Georacer]

During level flight, let's say that a lift L_0 is required.
During a coordinated turn with bank of angle phi, the lift required is L=L_0/cos(phi).

The induced drag is proportional to the square of lift.

Thus, the additional draft during a turn is proportional to
D-D_0=
L^2-L_0^2=
(L_0/cos(phi))^2 - L_0^2=
L_0^2 (1/cos(phi)^2 - 1)

which is what is currently implemented.

Could you amend the comment to reflect that?

[peterbarker]

Could you amend the comment to reflect that?

Done, thanks!