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

AP_TECS: correct comment explaining roll-induced-drag scaling 

Co-authored-by: George Zogopoulos <tailwhipx4@gmail.com>
This commit is contained in:
Peter Barker 2024-11-25 22:35:28 +11:00 committed by Peter Barker
parent 1ab0dcd45d
commit 9835fe80d6
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_TECS/AP_TECS.cpp
View File

@ -751,7 +751,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
const float nomThr = aparm.throttle_cruise * 0.01f; const float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); 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 // 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. // drag increase during turns.
const float cosPhi_squared = (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_squared, 0.1f, 1.0f) - 1.0f); 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 // Calculate additional throttle for turn drag compensation including throttle nudging
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); 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 // 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. // drag increase during turns.
const float cosPhi_squared = (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_squared, 0.1f, 1.0f) - 1.0f); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f);