Ignore:
Timestamp:
Feb 27, 2018, 12:20:03 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

add delta time to io_data

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairFilter/src/AhrsKalman_impl.cpp

    r167 r223  
    6565
    6666AhrsKalman_impl::AhrsKalman_impl(const Layout* layout,AhrsData *inAhrsData): ahrsData(inAhrsData) {
    67     is_init=false;
    6867    euler.yaw=0;
    6968
     
    9089    input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    9190
    92     delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    93     previous_time=data->DataTime();
     91    delta_t=(float)(data->DataDeltaTime())/1000000000.;
    9492
    9593    if(Q_angle->ValueChanged() || Q_gyro->ValueChanged() || R_angle->ValueChanged()) {
     
    9896    }
    9997
    100     if(is_init==true) {
     98    if(delta_t!=0) {
    10199        //execute kalman roll filter
    102100        ars_predict(&ars_roll, rawGyr.x, delta_t);
     
    116114        ahrsData->SetQuaternionAndAngularRates(euler.ToQuaternion(),
    117115                                               rawGyr-Vector3Df(ars_roll.x_bias,ars_pitch.x_bias,0));
    118     } else {
    119         is_init=true;
    120116    }
    121 
    122117    ahrsData->SetDataTime(data->DataTime());
    123118}
Note: See TracChangeset for help on using the changeset viewer.