Changeset 173 in flair-src


Ignore:
Timestamp:
Apr 26, 2017, 11:12:59 AM (5 years ago)
Author:
Sanahuja Guillaume
Message:

modif imu

Location:
trunk/lib
Files:
6 edited

Legend:

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

    r167 r173  
    3737Gx3_25_ahrs::~Gx3_25_ahrs() {}
    3838
    39 // datas from Gx3_25_imu are AhrsData!
     39// datas outputed from Gx3_25_imu are AhrsData!
    4040void Gx3_25_ahrs::UpdateFrom(const io_data *data) {
    4141  AhrsData *input = (AhrsData *)data;
     
    4646  Vector3Df filteredAngRates;
    4747  input->GetQuaternionAndAngularRates(quaternion, filteredAngRates);
    48   GetImu()->GetOneAxisRotation()->ComputeRotation(quaternion);
    49   GetImu()->GetOneAxisRotation()->ComputeRotation(filteredAngRates);
    5048  output->SetQuaternionAndAngularRates(quaternion, filteredAngRates);
    5149  output->SetDataTime(input->DataTime());
  • trunk/lib/FlairMeta/src/Uav.h

    r165 r173  
    6969  void SetBldc(const actuator::Bldc *bldc);
    7070  void SetMultiplex(const filter::UavMultiplex *multiplex);
    71   void SetAhrs(const filter::Ahrs *ahrs);
     71  void SetAhrs(const filter::Ahrs *ahrs);//also sets imu (retrieved from the ahrs)
    7272  void SetUsRangeFinder(const sensor::UsRangeFinder *us);
    7373  void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
  • 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  }
  • trunk/lib/FlairSensorActuator/src/Imu.cpp

    r167 r173  
    6464}
    6565
    66 void Imu::UpdateImu() {
     66void Imu::ApplyRotation(Vector3Df& vector) {
    6767  if (rotation == NULL) {
    6868    Err("not applicable for simulation part.\n");
    6969    return;
    7070  }
    71   Vector3Df rawAcc, rawMag, rawGyr;
    72   imuData->GetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
    73   rotation->ComputeRotation(rawAcc);
    74   rotation->ComputeRotation(rawGyr);
    75   rotation->ComputeRotation(rawMag);
    76   imuData->SetRawAccMagAndGyr(rawAcc, rawMag, rawGyr);
     71  rotation->ComputeRotation(vector);
     72}
     73
     74void Imu::ApplyRotation(Quaternion& quaternion) {
     75  if (rotation == NULL) {
     76    Err("not applicable for simulation part.\n");
     77    return;
     78  }
     79  rotation->ComputeRotation(quaternion);
    7780}
    7881
  • trunk/lib/FlairSensorActuator/src/Imu.h

    r137 r173  
    1515
    1616#include <IODevice.h>
     17#include <Vector3D.h>
    1718
    1819namespace flair {
     
    121122
    122123  /*!
    123   * \brief UpdateImu
     124  * \brief ApplyRotation
    124125  *
    125   * The reimplemented class must call this function as soon as IMU datas are
    126   *available. \n
     126  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
    127127  * It handles the data rotation if it was defined.
    128128  *
     129  * \param vector vector to apply rotation to
    129130  */
    130   void UpdateImu();
     131  void ApplyRotation(core::Vector3Df& vector);
     132 
     133  /*!
     134  * \brief ApplyRotation
     135  *
     136  * The reimplemented class must call this function to rotate IMU datas, before filling the ImuData. \n
     137  * It handles the data rotation if it was defined.
     138  *
     139  * \param quaternion quaternion to apply rotation to
     140  */
     141  void ApplyRotation(core::Quaternion& quaternion);
    131142
    132143  /*!
  • trunk/lib/FlairSensorActuator/src/SimuImu.cpp

    r167 r173  
    105105
    106106    shmem->Read((char *)&state, sizeof(imu_states_t));
    107     ahrsData->SetQuaternionAndAngularRates(
    108         Quaternion(state.q0, state.q1, state.q2, state.q3),
    109         Vector3Df(state.wx, state.wy, state.wz));
     107   
     108    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
     109    Vector3Df angRate(state.wx, state.wy, state.wz);
     110    ApplyRotation(angRate);
     111    ApplyRotation(quaternion);
     112    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
    110113
    111114    imuData->SetDataTime(GetTime());
    112115    ahrsData->SetDataTime(GetTime());
    113116
    114     UpdateImu();
    115117    ProcessUpdate(ahrsData);
    116118  }
Note: See TracChangeset for help on using the changeset viewer.