Ignore:
Timestamp:
04/26/17 11:12:59 (5 years ago)
Author:
Sanahuja Guillaume
Message:

modif imu

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r167 r173  
    2929#include <Vector3D.h>
    3030#include <RotationMatrix.h>
     31#include <OneAxisRotation.h>
    3132
    3233#include <math.h>
     
    135136      filteredAngRates.z = Dequeue(&buf);
    136137
    137       ahrsData->SetQuaternionAndAngularRates(eulerAngles.ToQuaternion(),
     138      Quaternion quaternion=eulerAngles.ToQuaternion();
     139      self->ApplyRotation(filteredAngRates);
     140      self->ApplyRotation(quaternion);
     141      ahrsData->SetQuaternionAndAngularRates(quaternion,
    138142                                             filteredAngRates);
    139143    } else if (command ==
     
    164168      matrix(2, 2) = Dequeue(&buf);
    165169
    166       ahrsData->SetQuaternionAndAngularRates(matrix.ToQuaternion(),
     170      Quaternion quaternion=matrix.ToQuaternion();
     171      self->ApplyRotation(filteredAngRates);
     172      self->ApplyRotation(quaternion);
     173      ahrsData->SetQuaternionAndAngularRates(quaternion,
    167174                                             filteredAngRates);
     175                                             
     176      self->ApplyRotation(rawAcc);
    168177      imuData->SetRawAcc(rawAcc);
    169178    } else if (command == Gx3_25_imu::Quaternion) {
     
    183192      filteredAngRates.z = 0;
    184193
     194      self->ApplyRotation(filteredAngRates);
     195      self->ApplyRotation(quaternion);
    185196      ahrsData->SetQuaternionAndAngularRates(quaternion,
    186197                                             filteredAngRates);
     
    207218    imuData->SetDataTime(imuTime);
    208219    ahrsData->SetDataTime(imuTime);
    209     self->UpdateImu();
    210220    self->ProcessUpdate(ahrsData);
    211221  }
Note: See TracChangeset for help on using the changeset viewer.