Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: remove unneccessary sqrtf in Phi calculations 

we take the square of this in the only use of it
This commit is contained in:
Peter Barker 2024-11-25 12:51:47 +11:00 committed by Peter Barker
parent b5ba09664e
commit 1ab0dcd45d
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_TECS/AP_TECS.cpp
View File

@ -753,8 +753,8 @@ void AP_TECS::_update_throttle_with_airspeed(void)
// Use the demanded rate of change of total energy as the feed-forward demand, but add // 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) - 1) to compensate for induced
// drag increase during turns. // drag increase during turns.
const float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); 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 * cosPhi, 0.1f, 1.0f) - 1.0f); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f);
const float ff_throttle = nomThr + STEdot_dem / K_thr2STE; const float ff_throttle = nomThr + STEdot_dem / K_thr2STE;
// Calculate PD + FF throttle // Calculate PD + FF throttle
@ -907,8 +907,8 @@ 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 // 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) - 1) to compensate for induced
// drag increase during turns. // drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); 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 * cosPhi, 0.1f, 1.0f) - 1.0f); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f);
_throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
constrain_throttle(); constrain_throttle();