Changeset 223 in flair-src for trunk/lib/FlairFilter


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

add delta time to io_data

Location:
trunk/lib/FlairFilter/src
Files:
16 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp

    r198 r223  
    3535namespace flair { namespace filter {
    3636
    37 AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): isInit(false), Ahrs(parent,name) {
     37AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): Ahrs(parent,name) {
    3838
    3939  QHat.q0=1;
    … …  
    7676    input->GetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    7777
    78     delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    79     previous_time=data->DataTime();
     78    delta_t=(float)(data->DataDeltaTime())/1000000000.;
    8079
    8180    Vector3Df aBar,aHat,aTilde;
    … …  
    8988    magRef=this->magRef->Value();
    9089
    91     if(isInit==true) {
     90    if(delta_t!=0) {
    9291        // CORRECTION FROM ACCELEROMETER
    9392        aBar = rawAcc;
    … …  
    153152        ahrsData->SetQuaternionAndAngularRates(QHat,rawGyr - BHat);
    154153
    155     } else {
    156         isInit=true;
    157     }
     154    }
    158155
    159156    ahrsData->SetDataTime(data->DataTime());

  • trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h

    r198 r223  
    6363            void UpdateFrom(const core::io_data *data);
    6464
    65             core::Time previous_time;
    66 
    67             bool isInit;
    6865            core::Quaternion QHat;
    6966            core::Vector3Df BHat;

  • 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}

  • trunk/lib/FlairFilter/src/ButterworthLowPass_impl.cpp

    r214 r223  
    5656  }
    5757
    58   first_update = true;
    5958  this->order=order;
    6059  this->nbRow=nbRow;
    … …  
    9190
    9291  if (T->Value() == 0) {
    93     delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     92    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    9493    for(uint32_t i=0;i<nbRow;i++) {
    9594      for(uint32_t j=0;j<nbCol;j++) {
    … …  
    114113  }
    115114
    116   if (first_update == true) {
    117     first_update = false;
    118   } else {
     115  if (delta_t!=0) {
    119116    for(uint32_t i=0;i<nbRow;i++) {
    120117      for(uint32_t j=0;j<nbCol;j++) {
    … …  
    129126
    130127  output->SetDataTime(data->DataTime());
    131   previous_time = data->DataTime();
    132128}
    133129

  • trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp

    r214 r223  
    3232                                           string name,
    3333                                           const Matrix *init_value) {
    34   first_update = true;
    3534  this->self = self;
    3635 
    … …  
    9493  } else {
    9594    if (T->Value() == 0) {
    96       delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     95      delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    9796    } else {
    9897      delta_t = T->Value();
    9998    }
    10099
    101     for (int i = 0; i < input->Rows(); i++) {
    102       for (int j = 0; j < input->Cols(); j++) {
    103         output->SetValueNoMutex(
    104             i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
    105                       delta_t);
    106         prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     100    if(delta_t!=0) {
     101      for (int i = 0; i < input->Rows(); i++) {
     102        for (int j = 0; j < input->Cols(); j++) {
     103          output->SetValueNoMutex(
     104              i, j, (input->ValueNoMutex(i, j) - prev_value->ValueNoMutex(i, j)) /
     105                        delta_t);
     106          prev_value->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     107        }
    107108      }
    108109    }
    … …  
    113114
    114115  output->SetDataTime(data->DataTime());
    115   previous_time = data->DataTime();
    116116}

  • trunk/lib/FlairFilter/src/JoyReference_impl.cpp

    r214 r223  
    6969
    7070  z_ref = 0;
    71   previous_time = 0;
    7271  trim_roll = 0;
    7372  trim_pitch = 0;
    … …  
    154153  }
    155154
    156   if (previous_time == 0)
    157     previous_time = data->DataTime(); // pour la premiere iteration
    158   float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
    159   previous_time = data->DataTime();
     155  float delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    160156
    161157  if (button_roll->Clicked() == true) {

  • trunk/lib/FlairFilter/src/LowPassFilter_impl.cpp

    r214 r223  
    6767                           " Hz", 0, 10000, 0.1, 2, 1);
    6868                           
    69   previous_time=GetTime();
    7069  this->self = self;
    7170}
    … …  
    8786
    8887  if (T->Value() == 0) {
    89     delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     88    delta_t = (float)(data->DataDeltaTime() ) / 1000000000.;
    9089  } else {
    9190    delta_t = T->Value();
    9291  }
    93   for (int i = 0; i < input->Rows(); i++) {
    94     for (int j = 0; j < input->Cols(); j++) {
    95       float cutoff=freq->Value();
    96       if (cutoff == 0 || freq->ValueChanged()) {
    97         output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
    98       } else {
    99         output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
    100                                               output->ValueNoMutex(i, j) +
    101                                           2 * PI * cutoff * delta_t *
    102                                               input->ValueNoMutex(i, j));
     92 
     93  if(delta_t!=0) {
     94    for (int i = 0; i < input->Rows(); i++) {
     95      for (int j = 0; j < input->Cols(); j++) {
     96        float cutoff=freq->Value();
     97        if (cutoff == 0 || freq->ValueChanged()) {
     98          output->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
     99        } else {
     100          output->SetValueNoMutex(i, j, (1 - 2 * PI * cutoff * delta_t) *
     101                                                output->ValueNoMutex(i, j) +
     102                                            2 * PI * cutoff * delta_t *
     103                                                input->ValueNoMutex(i, j));
     104        }
    103105      }
    104106    }
    … …  
    109111
    110112  output->SetDataTime(data->DataTime());
    111   previous_time = data->DataTime();
    112113}

  • trunk/lib/FlairFilter/src/PidThrust_impl.cpp

    r214 r223  
    8484
    8585  if (T->Value() == 0) {
    86     delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     86    delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
    8787  } else {
    8888    delta_t = T->Value();
    … …  
    121121  self->output->SetValue(0, 0, total - offset_g * offset_g);
    122122  self->output->SetDataTime(data->DataTime());
    123 
    124   previous_time = data->DataTime();
    125123}

  • trunk/lib/FlairFilter/src/Pid_impl.cpp

    r214 r223  
    7979
    8080  if (T->Value() == 0) {
    81     delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     81    delta_t = (float)(data->DataDeltaTime() ) / 1000000000.;
    8282  } else {
    8383    delta_t = T->Value();
    … …  
    113113  self->output->SetValue(0, 0, total);
    114114  self->output->SetDataTime(data->DataTime());
    115 
    116   previous_time = data->DataTime();
    117115}

  • trunk/lib/FlairFilter/src/unexported/AhrsKalman_impl.h

    r18 r223  
    7979        ars_Gyro1DKalman ars_roll;
    8080        ars_Gyro1DKalman ars_pitch;
    81         flair::core::Time previous_time;
    82         bool is_init;
    8381        flair::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle;
    8482        flair::core::Euler euler;

  • trunk/lib/FlairFilter/src/unexported/ButterworthLowPass_impl.h

    r214 r223  
    131131  flair::gui::DoubleSpinBox *cutoff, *T;
    132132  PoleFilter **f;
    133   bool first_update;
    134   flair::core::Time previous_time;
    135133  uint32_t order;
    136134  uint32_t nbRow, nbCol;

  • trunk/lib/FlairFilter/src/unexported/EulerDerivative_impl.h

    r214 r223  
    4545private:
    4646  flair::gui::DoubleSpinBox *T;
    47   flair::core::Time previous_time;
    4847  bool first_update;
    4948  flair::core::Matrix *prev_value;

  • trunk/lib/FlairFilter/src/unexported/JoyReference_impl.h

    r214 r223  
    8080  flair::core::Quaternion q_z = flair::core::Quaternion(1, 0, 0, 0);
    8181  float trim_roll, trim_pitch;
    82   flair::core::Time previous_time;
    8382
    8483  flair::filter::JoyReference *self;

  • trunk/lib/FlairFilter/src/unexported/LowPassFilter_impl.h

    r214 r223  
    4242
    4343private:
    44   flair::core::Time previous_time;
    4544  flair::gui::DoubleSpinBox *freq, *T;
    4645  const flair::filter::LowPassFilter *self;

  • trunk/lib/FlairFilter/src/unexported/PidThrust_impl.h

    r214 r223  
    4646private:
    4747  flair::filter::PidThrust *self;
    48   flair::core::Time previous_time;
    4948  bool first_update;
    5049

  • trunk/lib/FlairFilter/src/unexported/Pid_impl.h

    r214 r223  
    4646private:
    4747  flair::filter::Pid *self;
    48   flair::core::Time previous_time;
    4948
    5049  // matrix
Note: See TracChangeset for help on using the changeset viewer.