Ignore:
Timestamp:
06/23/17 17:33:03 (5 years ago)
Author:
Sanahuja Guillaume
Message:

maj imu

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r173 r186  
    8080  Time imuTime;
    8181  ImuData *imuData;
     82  Quaternion prevQuat;prevQuat.q0=-2;//for init
    8283  self->GetDatas(&imuData);
    8384
     
    115116  }
    116117
    117   // periode a passer en argument (reglable)
    118   // ou plutot laisser la periode geree par le centrale (polling)
    119   // self->SetPeriodMS(2);
    120118  SetContinuousMode(command);
    121119
     
    169167
    170168      Quaternion quaternion=matrix.ToQuaternion();
     169     
     170      //try to keep quaternion continuous (select between quaternion or -quaternion)
     171      if(prevQuat.q0!=-2) {//skip first iteration
     172        //search the greatest absolute value to avoid problems near 0 (quaternion's norm=1)
     173        if(fabs(prevQuat.q0)>=fabs(prevQuat.q1) && fabs(prevQuat.q0)>=fabs(prevQuat.q2) && fabs(prevQuat.q0)>=fabs(prevQuat.q3)) {
     174          if(prevQuat.q0*quaternion.q0<0) {//if signs are different
     175            quaternion=-quaternion;
     176          }
     177        } else if(fabs(prevQuat.q1)>=fabs(prevQuat.q0) && fabs(prevQuat.q1)>=fabs(prevQuat.q2) && fabs(prevQuat.q1)>=fabs(prevQuat.q3)) {
     178          if(prevQuat.q1*quaternion.q1<0) {//if signs are different
     179            quaternion=-quaternion;
     180          }
     181        } else if(fabs(prevQuat.q2)>=fabs(prevQuat.q0) && fabs(prevQuat.q2)>=fabs(prevQuat.q1) && fabs(prevQuat.q2)>=fabs(prevQuat.q3)) {
     182          if(prevQuat.q2*quaternion.q2<0) {//if signs are different
     183            quaternion=-quaternion;
     184          }
     185        } else if(fabs(prevQuat.q3)>=fabs(prevQuat.q0) && fabs(prevQuat.q3)>=fabs(prevQuat.q1) && fabs(prevQuat.q3)>=fabs(prevQuat.q2)) {
     186          if(prevQuat.q3*quaternion.q3<0) {//if signs are different
     187            quaternion=-quaternion;
     188          }
     189        }
     190      }
     191      prevQuat=quaternion;
     192     
    171193      self->ApplyRotation(filteredAngRates);
    172194      self->ApplyRotation(quaternion);
Note: See TracChangeset for help on using the changeset viewer.