void updateMagnetometer()
{
short compass_data[3] ;
mpu_get_compass_reg(compass_data,0) ;
short x , y, z ;
short X , Y, Z ;
x = compass_data[0] ;
y = compass_data[1] ;
z = compass_data[2] ;
X = -y ;
Y = -x ;
Z = z ;
short xMid = globalAttitudeSensor.MagOffset[0] ;
short yMid = globalAttitudeSensor.MagOffset[1] ;
short zMid = globalAttitudeSensor.MagOffset[2] ;
float ratioX = globalAttitudeSensor.MagRatio[0] ;
float ratioY = globalAttitudeSensor.MagRatio[1] ;
float ratioZ = globalAttitudeSensor.MagRatio[2] ;
globalAttitudeSensor.mx = (X-xMid)*ratioX ;
globalAttitudeSensor.my = (Y-yMid)*ratioY ;
globalAttitudeSensor.mz = (Z-zMid)*ratioZ ;
}
int mpulib_read_mag(mpudata_t *mpu) {
if (mpu_get_compass_reg(mpu->rawMag, &mpu->magTimestamp) < 0) {
printf("mpu_get_compass_reg() failed\n");
return -1;
}
return 0;
}
void MPU9250Device::getRawMagnetometer(short *values) {
short mag[3];
unsigned char magSampled = mpu_get_compass_reg(mag);
if(magSampled == 0) {
values[0] = mag[0];
values[1] = mag[1];
values[2] = mag[2];
}
}
void MPU9250Device::getMagnetometer(float *values) {
short mag[3];
unsigned char magSampled = mpu_get_compass_reg(mag);
if(magSampled == 0) {
values[0] = (float)(mag[0] - this->correctionVector.x) / MAG_SCALEFACTOR;
values[1] = (float)(mag[1] - this->correctionVector.y) / MAG_SCALEFACTOR;
values[2] = (float)(mag[2] - this->correctionVector.z) / MAG_SCALEFACTOR;
}
}
int mpu9150_read_mag(mpudata_t *mpu)
{
int error_code = 0;
if ((error_code = mpu_get_compass_reg(mpu->rawMag, &mpu->magTimestamp)) < 0) {
printk(KERN_WARNING "mpu_get_compass_reg() failed. Error %d\n", error_code);
sprintf(status_string,"dmp_read_fifo() failed\n");
return -1;
}
return 0;
}
int MPU9250_DMP::updateCompass(void)
{
short data[3];
if (mpu_get_compass_reg(data, &time))
{
return INV_ERROR;
}
mx = data[X_AXIS];
my = data[Y_AXIS];
mz = data[Z_AXIS];
return INV_SUCCESS;
}
int ms_update() {
if (!dmpReady) {
printf("Error: DMP not ready!!\n");
return -1;
}
while (dmp_read_fifo(g,a,_q,&sensors,&fifoCount)!=0); //gyro and accel can be null because of being disabled in the efeatures
q = _q;
GetGravity(&gravity, &q);
GetYawPitchRoll(ypr, &q, &gravity);
mpu_get_temperature(&t);
temp=(float)t/65536L;
mpu_get_compass_reg(c);
//scaling for degrees output
for (int i=0;i<DIM;i++){
ypr[i]*=180/M_PI;
}
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of Pitch, MPU is attached upside down
ypr[1]*=-1.0;
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
accel[i] = (float)(a[DIM-i-1]);
compass[i] = (float)(c[DIM-i-1]);
}
return 0;
}
boolean MPU9150Lib::read()
{
short intStatus;
int result;
short sensors;
unsigned char more;
unsigned long timestamp;
mpu_get_int_status(&intStatus); // get the current MPU state
if ((intStatus & MPU_INT_STATUS_DMP_0) == 0) // return false if definitely not ready yet
return false;
// get the data from the fifo
if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, ×tamp, &sensors, &more)) != 0) {
return false;
}
// got the fifo data so now get the mag data
if ((result = mpu_get_compass_reg(m_rawMag, ×tamp)) != 0) {
#ifdef MPULIB_DEBUG
Serial.print("Failed to read compass: ");
Serial.println(result);
return false;
#endif
}
// got the raw data - now process
m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W]; // get float version of quaternion
m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
MPUQuaternionNormalize(m_dmpQuaternion); // and normalize
MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
// *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
if (m_useMagCalibration) {
m_calMag[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
}
else {
m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
}
// Scale accel data
if (m_useAccelCalibration) {
m_calAccel[VEC3_X] = -(short)(((long)m_rawAccel[VEC3_X] * (long)SENSOR_RANGE) / (long)m_accelXRange);
m_calAccel[VEC3_Y] = (short)(((long)m_rawAccel[VEC3_Y] * (long)SENSOR_RANGE) / (long)m_accelYRange);
m_calAccel[VEC3_Z] = (short)(((long)m_rawAccel[VEC3_Z] * (long)SENSOR_RANGE) / (long)m_accelZRange);
}
else {
m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
}
dataFusion();
return true;
}
void SerialInterface::readData(float deltaTime) {
#ifndef _WIN32
int initialSamples = totalSamples;
if (USING_INVENSENSE_MPU9150) {
// ask the invensense for raw gyro data
short accelData[3];
if (mpu_get_accel_reg(accelData, 0)) {
close(_serialDescriptor);
qDebug("Disconnected SerialUSB.\n");
_active = false;
return; // disconnected
}
const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2;
short gyroData[3];
mpu_get_gyro_reg(gyroData, 0);
// Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
glm::vec3 rotationRates;
rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND;
short compassData[3];
mpu_get_compass_reg(compassData, 0);
// Convert integer values to floats, update extents
_lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]);
// update and subtract the long term average
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
rotationRates -= _averageRotationRates;
// compute the angular acceleration
glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
_lastRotationRates = rotationRates;
// Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates));
// Update acceleration estimate: first, subtract gravity as rotated into current frame
_estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
_lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// update and subtract the long term average
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
_estimatedAcceleration -= _averageAcceleration;
// Consider updating our angular velocity/acceleration to linear acceleration mapping
if (glm::length(_estimatedAcceleration) > EPSILON &&
(glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
// compute predicted linear acceleration, find error between actual and predicted
glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
_angularAccelToLinearAccel * angularAcceleration;
glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
// the "error" is actually what we want: the linear acceleration minus rotational influences
_estimatedAcceleration = error;
// adjust according to error in each dimension, in proportion to input magnitudes
for (int i = 0; i < 3; i++) {
if (fabsf(error[i]) < EPSILON) {
continue;
}
const float LEARNING_RATE = 0.001f;
float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
if (rateSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
if (proportion > EPSILON) {
_angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
}
}
}
float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
if (accelSum > EPSILON) {
for (int j = 0; j < 3; j++) {
float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
if (proportion > EPSILON) {
_angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
}
}
}
}
}
// rotate estimated acceleration into global rotation frame
_estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.975f;
float const DECAY_POSITION = 0.975f;
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
// Attempt to fuse gyro position with webcam position
Webcam* webcam = Application::getInstance()->getWebcam();
if (webcam->isActive()) {
const float WEBCAM_POSITION_FUSION = 0.5f;
_estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
} else {
_estimatedPosition *= DECAY_POSITION;
}
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_gravity = _lastAcceleration;
}
else {
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES);
// North samples start later, because the initial compass readings are screwy
int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES);
if (northSample == 0) {
_north = _lastCompass;
} else if (northSample > 0) {
_north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES);
}
} else {
// Use gravity reading to do sensor fusion on the pitch and roll estimation
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES);
// Update the compass extents
_compassMinima = glm::min(_compassMinima, _lastCompass);
_compassMaxima = glm::max(_compassMaxima, _lastCompass);
// Same deal with the compass heading
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * recenterCompass(_lastCompass),
recenterCompass(_north)) * estimatedRotation,
1.0f / COMPASS_SENSOR_FUSION_SAMPLES);
}
}
_estimatedRotation = safeEulerAngles(estimatedRotation);
totalSamples++;
}
if (initialSamples == totalSamples) {
timeval now;
gettimeofday(&now, NULL);
if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
qDebug("No data - Shutting down SerialInterface.\n");
resetSerial();
}
} else {
gettimeofday(&lastGoodRead, NULL);
}
#endif
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
SysTick_Config(SystemCoreClock / 1000);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_ResetBits(GPIOE, GPIO_Pin_2); //start with gps off to make sure it activates when wanted
GPIO_Start();
ADC_Start();
Flash_Start();
unsigned long tickey = getSysTick()+1000;
GPIO_ResetBits(GPIOA, GPIO_Pin_10); //LCD Reset must be held 10us
GPIO_SetBits(GPIOG, GPIO_Pin_3); //flash deselect
GPIO_SetBits(GPIOC, GPIO_Pin_8); //flash #hold off, we have dedicated pins
GPIO_SetBits(GPIOC, GPIO_Pin_1); //osc enable
GPIO_ResetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOE, GPIO_Pin_6); //buck enable
while(getSysTick()<tickey);
GPIO_SetBits(GPIOE, GPIO_Pin_2); //gps on/off
GPIO_SetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOA, GPIO_Pin_10); //LCD unreset
UART4_Start();
UART5_Start();
MPU_Start();
//========================BUTTONS====================
InitButton(&button1, GPIOE, GPIO_Pin_4);
#ifdef BOARD_V1
InitButton(&button2, GPIOE, GPIO_Pin_5);
#else
InitButton(&button2, GPIOA, GPIO_Pin_9);
#endif
//=======================END BUTTONS==================
/* LCD Configuration */
LCD_Config();
/* Enable The LCD */
LTDC_Cmd(ENABLE);
LCD_SetLayer(LCD_FOREGROUND_LAYER);
GUI_ClearBackground();
int count = 0;
delay(20000);
#ifndef ORIGIN
GUI_InitNode(1, 72, 83, 0xe8ec);
GUI_InitNode(2, 86, 72, 0xfd20);
GUI_InitNode(3, 'R', 'F', 0x001f);
#endif
int screencount = 0;
#ifdef INSIDE
origin_state.lati=KRESGE_LAT;
origin_state.longi=KRESGE_LONG;
origin_state.gpslock=1;
#endif
unsigned long tickey2 = getSysTick()+2000; //2 second counter
unsigned long tickey3 = getSysTick()+4000; //4 second delay to check gps state
/* Infinite loop */
while (1)
{
UpdateButton(&button1);
UpdateButton(&button2);
if( buttonRisingEdge(&button1)){//right
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);//yellow
//UART_Transmit(&huart4, gps_init_msg, cmdData1Len, 500);
origin_state.pingnum+=1;
origin_state.pingactive=1;
origin_state.whodunnit = origin_state.id;
origin_state.pingclearedby = 0;
}
if(buttonRisingEdge(&button2)){//left
//UART_Transmit(&huart4, gps_get_time_msg, cmdData2Len, 500);
GPIO_ToggleBits(GPIOA, GPIO_Pin_2); //green
if(origin_state.pingactive&&(origin_state.whodunnit != origin_state.id)){
origin_state.pingactive=0;
}
}
if(origin_state.gpson>2 &&(getSysTick()>tickey3)){
GPIO_ResetBits(GPIOE, GPIO_Pin_2);
delay(20000);
GPIO_SetBits(GPIOE, GPIO_Pin_2);
delay(20000);
char setme[80];
sprintf(setme, "%s%c%c", gps_init_msg, 0x0D, 0x0A);
UART_Transmit(UART4, setme, sizeof(setme)/sizeof(setme[0]), 5000);
origin_state.gpson=0;
tickey3+=4000;
}
if(getReset()){
NVIC_SystemReset();
}
#ifdef ORIGIN
// long actHeading=0;
// inv_get_sensor_type_heading(&actHeading, &headingAcc, &headingTime);
// degrees=((double)actHeading)/((double)65536.0);
// origin_state.heading=degrees;
long actHeading[3] = {0,0,0};
inv_get_sensor_type_euler(actHeading, &headingAcc, &headingTime);
degrees=((double)actHeading[2])/((double)65536.0);
//origin_state.heading=degrees;
long tempyraiture;
mpu_get_temperature(&tempyraiture, NULL);
// short garbage[3];
// mpu_get_compass_reg(garbage, NULL);
// double compass_angle = atan2(-garbage[0], -garbage[1])*180/3.1415;
// //origin_state.heading = .9*degrees + .1*compass_angle;
// origin_state.heading = compass_angle;
#endif
if(getSysTick()>tickey2){
tickey2 +=2000;
sendMessage();
}
processGPS();
processXbee();
if(getSysTick()>tickey){
tickey +=53;
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);
#ifndef ORIGIN
GUI_UpdateNode(1, degrees*3.1415/180.0+3.14*1.25, screencount, (screencount>10), 0);
GUI_UpdateNode(2, degrees*3.1415/180.0+3.14, screencount, (screencount>30), 0);
GUI_UpdateNode(3, degrees*3.1415/180.0+0, screencount, (screencount>50), 0);
#else
GUI_UpdateNodes();
#endif
GUI_UpdateArrow(-degrees*3.1415/180.0);
GUI_UpdateBattery(getBatteryStatus());
GUI_DrawTime();
if (count > 50){
GUI_UpdateBottomButton(1, 0xe8ec);
} else {
GUI_UpdateBottomButton(0, 0);
}
GUI_Redraw();
screencount += 1;
#ifndef ORIGIN
degrees += 3.6;
if (screencount%100 == 0){
screencount = 0;
degrees = 0;
}
#else
if (screencount%100 == 0){
screencount = 0;
}
#endif
}
//Sensors_I2C_ReadRegister((unsigned char)0x68, (unsigned char)MPU_WHOAMI, 1, inImu);
//==================================IMU================================
unsigned long sensor_timestamp;
int new_data = 0;
get_tick_count(×tamp);
#ifdef COMPASS_ENABLED
/* We're not using a data ready interrupt for the compass, so we'll
* make our compass reads timer-based instead.
*/
if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
hal.new_gyro && (hal.sensors & COMPASS_ON)) {
hal.next_compass_ms = timestamp + COMPASS_READ_MS;
new_compass = 1;
}
#endif
/* Temperature data doesn't need to be read with every gyro sample.
* Let's make them timer-based like the compass reads.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.motion_int_mode) {
/* Enable motion interrupt. */
mpu_lp_motion_interrupt(500, 1, 5);
/* Notify the MPL that contiguity was broken. */
inv_accel_was_turned_off();
inv_gyro_was_turned_off();
inv_compass_was_turned_off();
inv_quaternion_sensor_was_turned_off();
/* Wait for the MPU interrupt. */
while (!hal.new_gyro) {}
/* Restore the previous sensor configuration. */
mpu_lp_motion_interrupt(0, 0, 0);
hal.motion_int_mode = 0;
}
if (!hal.sensors || !hal.new_gyro) {
continue;
}
if (hal.new_gyro && hal.lp_accel_mode) {
short accel_short[3];
long accel[3];
mpu_get_accel_reg(accel_short, &sensor_timestamp);
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
hal.new_gyro = 0;
} else if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
inv_build_quat(quat, 0, sensor_timestamp);
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel_short[3];
unsigned char sensors, more;
long accel[3], temperature;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
}
#ifdef COMPASS_ENABLED
if (new_compass) {
short compass_short[3];
long compass[3];
new_compass = 0;
/* For any MPU device with an AKM on the auxiliary I2C bus, the raw
* magnetometer registers are copied to special gyro registers.
*/
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
compass[0] = (long)compass_short[0];
compass[1] = (long)compass_short[1];
compass[2] = (long)compass_short[2];
/* NOTE: If using a third-party compass calibration library,
* pass in the compass data in uT * 2^16 and set the second
* parameter to INV_CALIBRATED | acc, where acc is the
* accuracy from 0 to 3.
*/
inv_build_compass(compass, 0, sensor_timestamp);
}
new_data = 1;
}
#endif
if (new_data) {
inv_execute_on_data();
/* This function reads bias-compensated sensor data and sensor
* fusion outputs from the MPL. The outputs are formatted as seen
* in eMPL_outputs.c. This function only needs to be called at the
* rate requested by the host.
*/
read_from_mpl();
}
//========================================IMU==================================
}
}
