Changeset 223 in flair-src


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
Files:
22 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairCore/src/io_data.cpp

    r51 r223  
    4646void io_data::SetDataTime(Time time) {
    4747  GetMutex();
     48  if(pimpl_->time!=TIME_INFINITE) pimpl_->deltaTime = time-pimpl_->time;
     49  pimpl_->time = time;
     50  ReleaseMutex();
     51}
     52
     53void io_data::SetDataTime(Time time,Time deltaTime) {
     54  GetMutex();
     55  pimpl_->deltaTime = deltaTime;
    4856  pimpl_->time = time;
    4957  ReleaseMutex();
    … …  
    5664  ReleaseMutex();
    5765  return tmp;
     66}
     67
     68Time io_data::DataDeltaTime(void) const {
     69  Time tmp;
     70  GetMutex();
     71  tmp = pimpl_->deltaTime;
     72  ReleaseMutex();
     73  return tmp;
     74}
     75
     76void io_data::GetDataTime(Time &time,Time &deltaTime) const{
     77  GetMutex();
     78  deltaTime=pimpl_->deltaTime ;
     79  time=pimpl_->time ;
     80  ReleaseMutex();
    5881}
    5982

  • trunk/lib/FlairCore/src/io_data.h

    r51 r223  
    116116
    117117  /*!
    118   * \brief Set data time
    119   *
     118  * \brief Set data time, also caluculates the delta time based on last call
     119  *
     120  * TIME_INFINITE represents an unitialized time
     121  *
    120122  * \param time time
    121123  */
    122124  void SetDataTime(Time time);
     125 
     126  /*!
     127  * \brief Set data time and  delta time (thus delta time is not based on last call)
     128  *
     129  * TIME_INFINITE represents an unitialized time
     130  *
     131  * \param time time
     132  * \param deltaTime delta time
     133  */
     134  void SetDataTime(Time time,Time deltaTime);
    123135
    124136  /*!
    125137  * \brief Data time
    126138  *
     139  * TIME_INFINITE represents an unitialized time.
     140  *
    127141  * \return data time
    128142  */
    129143  Time DataTime(void) const;
     144 
     145  /*!
     146  * \brief Data delta time
     147  *
     148  * TIME_INFINITE represents an unitialized time.
     149  *
     150  * \return data delta time
     151  */
     152  Time DataDeltaTime(void) const;
     153 
     154  /*!
     155  * \brief Get data time and  delta time
     156  *
     157  * TIME_INFINITE represents an unitialized time
     158  *
     159  * \param time time
     160  * \param deltaTime delta time
     161  */
     162  void GetDataTime(Time &time,Time &deltaTime) const;
    130163
    131164  /*!

  • trunk/lib/FlairCore/src/io_data_impl.cpp

    r122 r223  
    3131  is_consistent = false;
    3232  circle_ptr = NULL;
     33  deltaTime=TIME_INFINITE;//init
     34  time=TIME_INFINITE;//init
    3335}
    3436

  • trunk/lib/FlairCore/src/unexported/io_data_impl.h

    r15 r223  
    3737                            flair::core::DataType const &datatype);
    3838  size_t size;
    39   flair::core::Time time;
     39  flair::core::Time time,deltaTime;
    4040  void **circle_ptr;
    4141

  • 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

  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r218 r223  
    9393    parent->pimpl_->AddTrackable(this);
    9494  }
    95  
     95 
     96  previousTime=TIME_INFINITE;
    9697}
    9798
    … …  
    143144  Time time = GetTime();
    144145  //Printf("%s %lld %lld\n",caller->self->ObjectName().c_str(),time,t.msg_time.tv_sec*1000000+t.msg_time.tv_usec);
    145 
     146  Time vrpnTime=t.msg_time.tv_sec*1000000000+t.msg_time.tv_usec*1000;
     147  Time deltaTime;
     148  if(caller->previousTime!=TIME_INFINITE) {
     149    deltaTime=vrpnTime-caller->previousTime;
     150  } else {
     151    deltaTime=TIME_INFINITE;
     152  }
     153  caller->previousTime=vrpnTime;
     154 
    146155  // check if something is nan
    147156  for (int i = 0; i < 3; i++) {
    … …  
    178187  caller->output->SetValueNoMutex(6, 0, pos.z);
    179188
    180   caller->output->SetDataTime(time);
     189  caller->output->SetDataTime(time,deltaTime);
    181190  caller->output->ReleaseMutex();
    182191

  • trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h

    r214 r223  
    6767  vrpn_Tracker_Remote *tracker;
    6868  void Update(void);
     69  flair::core::Time previousTime;
    6970};
    7071
Note: See TracChangeset for help on using the changeset viewer.