Changeset 186 in flair-src for trunk/lib/FlairSensorActuator/src
- Timestamp:
- Jun 23, 2017, 5:33:03 PM (7 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 6 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp
r173 r186 80 80 Time imuTime; 81 81 ImuData *imuData; 82 Quaternion prevQuat;prevQuat.q0=-2;//for init 82 83 self->GetDatas(&imuData); 83 84 … … 115 116 } 116 117 117 // periode a passer en argument (reglable)118 // ou plutot laisser la periode geree par le centrale (polling)119 // self->SetPeriodMS(2);120 118 SetContinuousMode(command); 121 119 … … 169 167 170 168 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 171 193 self->ApplyRotation(filteredAngRates); 172 194 self->ApplyRotation(quaternion); -
trunk/lib/FlairSensorActuator/src/Imu.cpp
r179 r186 42 42 setupGroupbox = new GroupBox(sensorTab->NewRow(), name); 43 43 rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation"); 44 AddDataToLog(imuData); 44 45 } 45 46 -
trunk/lib/FlairSensorActuator/src/Mb800.cpp
r180 r186 108 108 109 109 while (!ToBeStopped()) { 110 SleepMS(10);110 //SleepMS(10);?? 111 111 size = 0; 112 112 while (!ToBeStopped()) { -
trunk/lib/FlairSensorActuator/src/SimuImu.cpp
r173 r186 79 79 state.wy = input->ValueNoMutex(8, 0); 80 80 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); 81 87 input->ReleaseMutex(); 82 88 … … 108 114 Quaternion quaternion(state.q0, state.q1, state.q2, state.q3); 109 115 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); 110 119 ApplyRotation(angRate); 111 120 ApplyRotation(quaternion); 121 ApplyRotation(rawAcc); 122 ApplyRotation(rawMag); 123 ApplyRotation(rawGyr); 112 124 ahrsData->SetQuaternionAndAngularRates(quaternion,angRate); 113 125 imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr); 114 126 imuData->SetDataTime(GetTime()); 115 127 ahrsData->SetDataTime(GetTime()); -
trunk/lib/FlairSensorActuator/src/SimuImu.h
r158 r186 94 94 float wy; 95 95 float wz; 96 float ax; 97 float ay; 98 float az; 99 float mx; 100 float my; 101 float mz; 96 102 } imu_states_t; 97 103 -
trunk/lib/FlairSensorActuator/src/TargetController.cpp
r137 r186 118 118 Message *message; 119 119 120 if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) { 120 //if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) { 121 if (!ControllerInitialization()) { 121 122 SafeStop(); 122 123 Thread::Err("An error occured, we don't proceed with the loop.\n");
Note:
See TracChangeset
for help on using the changeset viewer.