[Jsbsim-cvslogs] CVS: JSBSim/src/initialization FGTrim.cpp, 1.32, 1.33

[Bertrand <bco...@us...>]

Update of /cvsroot/jsbsim/JSBSim/src/initialization
In directory sfp-cvs-1.v30.ch3.sourceforge.com:/tmp/cvs-serv8237/src/initialization

Modified Files:
	FGTrim.cpp 
Log Message:
Fixed 2 bugs for the trim algorithm on ground. First, the algorithm was using the compressed contacts which was leading to incorrect placement once the aircraft was relocated with uncompressed contacts. Second, the order in which the rotations were applied was wrong and that was corrupting the trim solution for tail wheel aircrafts. Unfortunately the error was harmless for tricycle aircrafts which generally have small pitch angles when resting on ground.

Index: FGTrim.cpp
===================================================================
RCS file: /cvsroot/jsbsim/JSBSim/src/initialization/FGTrim.cpp,v
retrieving revision 1.32
retrieving revision 1.33
diff -C2 -r1.32 -r1.33
*** FGTrim.cpp	4 Jun 2016 19:04:20 -0000	1.32
--- FGTrim.cpp	5 Jun 2016 15:23:02 -0000	1.33
***************
*** 199,205 ****
    double PitchTrim0 = FCS->GetPitchTrimCmd();
  
!   for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
      fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
-   }
  
    fdmex->SetTrimStatus(true);
--- 199,204 ----
    double PitchTrim0 = FCS->GetPitchTrimCmd();
  
!   for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++)
      fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
  
    fdmex->SetTrimStatus(true);
***************
*** 345,351 ****
    fdmex->SetTrimStatus(false);
  
!   for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
      fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
-   }
  
    return !trim_failed;
--- 344,349 ----
    fdmex->SetTrimStatus(false);
  
!   for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++)
      fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
  
    return !trim_failed;
***************
*** 385,389 ****
    FGLocation CGLocation = Propagate->GetLocation();
    FGMatrix33 Tec2b = Propagate->GetTec2b();
!   FGMatrix33 Tl2b = Propagate->GetTl2b();
    double hmin = 1E+10;
    int contactRef = -1;
--- 383,387 ----
    FGLocation CGLocation = Propagate->GetLocation();
    FGMatrix33 Tec2b = Propagate->GetTec2b();
!   FGMatrix33 Tb2l = Propagate->GetTb2l();
    double hmin = 1E+10;
    int contactRef = -1;
***************
*** 391,400 ****
    // Build the list of the aircraft contact points and take opportunity of the
    // loop to find which one is closer to (or deeper into) the ground.
!   for (int i = 0; i < GroundReactions->GetNumGearUnits(); i++) {
      ContactPoints c;
!     FGLGear* gear = GroundReactions->GetGearUnit(i);
!     c.location = gear->GetLocalGear();
!     FGLocation gearLoc = CGLocation.LocalToLocation(c.location);
!     c.location = Tl2b * c.location;
  
      FGColumnVector3 normal, vDummy;
--- 389,396 ----
    // Build the list of the aircraft contact points and take opportunity of the
    // loop to find which one is closer to (or deeper into) the ground.
!   for (int i = 0; i < GroundReactions->GetNumGearUnits(); ++i) {
      ContactPoints c;
!     c.location = GroundReactions->GetGearUnit(i)->GetBodyLocation();
!     FGLocation gearLoc = CGLocation.LocalToLocation(Tb2l * c.location);
  
      FGColumnVector3 normal, vDummy;
***************
*** 439,443 ****
    FGMatrix33 rot = q0.GetTInv();
    vector<ContactPoints>::iterator iter;
!   for (iter = contacts.begin(); iter != contacts.end(); iter++)
      iter->location = contact0 + rot * (iter->location - contact0);
  
--- 435,439 ----
    FGMatrix33 rot = q0.GetTInv();
    vector<ContactPoints>::iterator iter;
!   for (iter = contacts.begin(); iter != contacts.end(); ++iter)
      iter->location = contact0 + rot * (iter->location - contact0);
  
***************
*** 463,467 ****
  
    // Update the aircraft orientation
!   FGColumnVector3 euler = (q0 * q1 * fgic.GetOrientation()).GetEuler();
  
    fgic.SetPhiRadIC(euler(1));
--- 459,463 ----
  
    // Update the aircraft orientation
!   FGColumnVector3 euler = (fgic.GetOrientation() * q0 * q1).GetEuler();
  
    fgic.SetPhiRadIC(euler(1));
***************
*** 488,492 ****
    rParam.angleMin = 3.0 * M_PI;
  
!   for (iter = contacts.begin(); iter != contacts.end(); iter++) {
      // Below the processed contact point is named 'M'
      // Construct an orthonormal basis (u, v, t). The ground normal is obtained
--- 484,488 ----
    rParam.angleMin = 3.0 * M_PI;
  
!   for (iter = contacts.begin(); iter != contacts.end(); ++iter) {
      // Below the processed contact point is named 'M'
      // Construct an orthonormal basis (u, v, t). The ground normal is obtained