Changeset 223 in flair-src for trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp

Timestamp:

02/27/18 12:20:03 (6 years ago)

Author:

Sanahuja Guillaume

Message:

add delta time to io_data

File:

• trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp (modified) (4 diffs)

trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp

@@ -35 +35 @@
 namespace flair { namespace filter {
 
-AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): isInit(false), Ahrs(parent,name) {
+AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): Ahrs(parent,name) {
 
   QHat.q0=1;
@@ -76 +76 @@
     input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
 
-    delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
-    previous_time=data->DataTime();
+    delta_t=(float)(data->DataDeltaTime())/1000000000.;
 
     Vector3Df aBar,aHat,aTilde;
@@ -89 +88 @@
     magRef=this->magRef->Value();
 
-    if(isInit==true) {
+    if(delta_t!=0) {
         // CORRECTION FROM ACCELEROMETER
         aBar = rawAcc;
@@ -153 +152 @@
         ahrsData->SetQuaternionAndAngularRates(QHat,rawGyr - BHat);
 
-    } else {
-        isInit=true;
-    }
+    } 
 
     ahrsData->SetDataTime(data->DataTime());