Changeset 102 in flair-src


Ignore:
Timestamp:
Oct 6, 2016, 11:21:23 AM (4 years ago)
Author:
Sanahuja Guillaume
Message:

3dmgx3 quaternion

Location:
trunk/lib/FlairSensorActuator/src
Files:
3 edited

Legend:

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

    r101 r102  
    3838  } else if (command == AccelerationAngularRateAndOrientationMatrix) {
    3939    //Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
     40  } else if (command == Quaternion) {
     41
    4042  } else {
    4143    Thread::Err("command not supported (%i)\n", command);
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h

    r15 r102  
    4444    AccelerationAngularRateAndOrientationMatrix =
    4545        0xc8, /*!< Acceleration, angular rate and orientation matrix */
     46    Quaternion = 0xdf, /*!< Quaternion */
    4647  };
    4748
  • trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

    r100 r102  
    113113  }
    114114
    115   // periode a passer an argument (reglable)
     115  // periode a passer en argument (reglable)
    116116  // ou plutot laisser la periode geree par le centrale (polling)
    117117  // self->SetPeriodMS(2);
     
    168168                                             filteredAngRates);
    169169      imuData->SetRawAcc(rawAcc);
     170    } else if (command == Gx3_25_imu::Quaternion) {
     171      uint8_t response[23] = {0};
     172      uint8_t *buf = &response[1];
     173      GetData(response, sizeof(response), &imuTime);
     174
     175      Quaternion quaternion;
     176      quaternion.q0 = Dequeue(&buf);
     177      quaternion.q1 = Dequeue(&buf);
     178      quaternion.q2 = Dequeue(&buf);
     179      quaternion.q3 = Dequeue(&buf);
     180
     181      Vector3D filteredAngRates;
     182      filteredAngRates.x = 0;
     183      filteredAngRates.y = 0;
     184      filteredAngRates.z = 0;
     185
     186      ahrsData->SetQuaternionAndAngularRates(quaternion,
     187                                             filteredAngRates);
    170188    }
    171189
Note: See TracChangeset for help on using the changeset viewer.