Changeset 186 in flair-src for trunk/lib/FlairSensorActuator


Ignore:
Timestamp:
Jun 23, 2017, 5:33:03 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

maj imu

Location:
trunk/lib/FlairSensorActuator/src
Files:
6 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);
  • trunk/lib/FlairSensorActuator/src/Imu.cpp

    r179 r186  
    4242  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
    4343  rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation");
     44  AddDataToLog(imuData);
    4445}
    4546
  • trunk/lib/FlairSensorActuator/src/Mb800.cpp

    r180 r186  
    108108
    109109  while (!ToBeStopped()) {
    110     SleepMS(10);
     110    //SleepMS(10);??
    111111    size = 0;
    112112    while (!ToBeStopped()) {
  • trunk/lib/FlairSensorActuator/src/SimuImu.cpp

    r173 r186  
    7979    state.wy = input->ValueNoMutex(8, 0);
    8080    state.wz = input->ValueNoMutex(9, 0);
     81    state.ax = input->ValueNoMutex(13, 0);
     82    state.ay = input->ValueNoMutex(14, 0);
     83    state.az = input->ValueNoMutex(15, 0);
     84    state.mx = input->ValueNoMutex(16, 0);
     85    state.my = input->ValueNoMutex(17, 0);
     86    state.mz = input->ValueNoMutex(18, 0);
    8187    input->ReleaseMutex();
    8288
     
    108114    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
    109115    Vector3Df angRate(state.wx, state.wy, state.wz);
     116    Vector3Df rawAcc(state.ax, state.ay, state.az);
     117    Vector3Df rawMag(state.mx, state.my, state.mz);
     118    Vector3Df rawGyr(state.wx, state.wy, state.wz);
    110119    ApplyRotation(angRate);
    111120    ApplyRotation(quaternion);
     121    ApplyRotation(rawAcc);
     122    ApplyRotation(rawMag);
     123    ApplyRotation(rawGyr);
    112124    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
    113 
     125    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    114126    imuData->SetDataTime(GetTime());
    115127    ahrsData->SetDataTime(GetTime());
  • trunk/lib/FlairSensorActuator/src/SimuImu.h

    r158 r186  
    9494    float wy;
    9595    float wz;
     96    float ax;
     97    float ay;
     98    float az;
     99    float mx;
     100    float my;
     101    float mz;
    96102  } imu_states_t;
    97103 
  • trunk/lib/FlairSensorActuator/src/TargetController.cpp

    r137 r186  
    118118  Message *message;
    119119
    120   if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     120  //if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     121  if (!ControllerInitialization()) {
    121122    SafeStop();
    122123    Thread::Err("An error occured, we don't proceed with the loop.\n");
Note: See TracChangeset for help on using the changeset viewer.