Changeset 223 in flair-src for trunk/lib/FlairFilter/src/AhrsKalman_impl.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/AhrsKalman_impl.cpp (modified) (4 diffs)

trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp

@@ -65 +65 @@
 
 AhrsKalman_impl::AhrsKalman_impl(const Layout* layout,AhrsData *inAhrsData): ahrsData(inAhrsData) {
-    is_init=false;
     euler.yaw=0;
 
@@ -90 +89 @@
     input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
 
-    delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
-    previous_time=data->DataTime();
+    delta_t=(float)(data->DataDeltaTime())/1000000000.;
 
     if(Q_angle->ValueChanged() || Q_gyro->ValueChanged() || R_angle->ValueChanged()) {
@@ -98 +96 @@
     }
 
-    if(is_init==true) {
+    if(delta_t!=0) {
         //execute kalman roll filter
         ars_predict(&ars_roll, rawGyr.x, delta_t);
@@ -116 +114 @@
         ahrsData->SetQuaternionAndAngularRates(euler.ToQuaternion(),
                                                rawGyr-Vector3Df(ars_roll.x_bias,ars_pitch.x_bias,0));
-    } else {
-        is_init=true;
     }
-
     ahrsData->SetDataTime(data->DataTime());
 }