PART 3
_accelScale = 16.0f / 32768.0f; // setting the accel scale to 16G
// ACCEL_CONFIG,ACCEL_FS_SEL_8G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_8G );
// _accelScale = 8.0f/32768.0f; // setting the accel scale to 8G
// ACCEL_CONFIG,ACCEL_FS_SEL_4G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_4G );
// _accelScale = 4.0f/32768.0f; // setting the accel scale to 4G
// ACCEL_CONFIG,ACCEL_FS_SEL_2G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_2G );
// _accelScale = 2.0f/32768.0f;
// // _accelScale = G * 2.0f / 32767.5f; // setting the accel scale to 2G
// set gyro scale
// GYRO_CONFIG,GYRO_FS_SEL_1000DPS
fWriteSPIdata8bits( GYRO_CONFIG, GYRO_FS_SEL_1000DPS );
// 250.0f/32768.0f;
// 500.0f/32768.0f;
// 2000.0f/32768.0f;
_gyroScale = 1000.0f / 32768.0f;
// read the AK8963 ASA registers and compute magnetometer scale factors
// set accel scale
fReadAK8963(AK8963_ASA, 3 );
fReadMPU9250 ( 3 , EXT_SENS_DATA_00 );
// convert to mG multiply by 10
_magScaleX = ((((float)rxData[0]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleY = ((((float)rxData[1]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleZ = ((((float)rxData[2]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
// set AK8963 to 16 bit resolution, 100 Hz update rate
// AK8963_CNTL1,AK8963_CNT_MEAS2
fWrite_AK8963( AK8963_CNTL1, AK8963_CNT_MEAS2 );
// delay for mode change
vTaskDelay ( 100 );
// AK8963_HXL,7 ;
fReadAK8963(AK8963_HXL, 7 );
////
/* setting the interrupt */
// INT_PIN_CFG,INT_PULSE_50US setup interrupt, 50 us pulse
fWriteSPIdata8bits( INT_PIN_CFG, INT_PULSE_50US );
// INT_ENABLE,INT_RAW_RDY_EN set to data ready
fWriteSPIdata8bits( INT_ENABLE, INT_RAW_RDY_EN );
pinMode( MPU_int_Pin, INPUT );
attachInterrupt( MPU_int_Pin, triggerGet_IMU, RISING );
}
}
////////////////////////////////////////////////////////////////
TickType_t TimePast = xTaskGetTickCount();
TickType_t TimeNow = xTaskGetTickCount();
TickType_t xLastWakeTime;
const TickType_t xFrequency = pdMS_TO_TICKS( 100 );
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
////
while (1)
{
xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
// vTaskDelayUntil( &xLastWakeTime, xFrequency );
// ACCEL_OUT
// ACCELX_OUT = 0x3B;
if ( MPU9250_OK && AK8963_OK )
{
TimeNow = xTaskGetTickCount();
deltat = ( (float)TimeNow - (float)TimePast) / 1000.0f;
////
fReadMPU9250 ( 8 , 0X3A );
_axcounts = (int16_t)(((int16_t)rxData[2] << 8) | rxData[3]) ;
_aycounts = (int16_t)(((int16_t)rxData[4] << 8) | rxData[5]) ;
_azcounts = (int16_t)(((int16_t)rxData[6] << 8) | rxData[7]) ;
fReadMPU9250 ( 6 , GYRO_OUTX );
_gxcounts = (int16_t)(((int16_t)rxData[0]) << 8) | rxData[1];
_gycounts = (int16_t)(((int16_t)rxData[2]) << 8) | rxData[3];
_gzcounts = (int16_t)(((int16_t)rxData[4]) << 8) | rxData[5];
// EXT_SENS_DATA_00
fReadMPU9250 ( 6 , EXT_SENS_DATA_00 );
_hxcounts = ((int16_t)rxData[1] << 8) | rxData[0] ;
_hycounts = ((int16_t)rxData[3] << 8) | rxData[2] ;;
_hzcounts = ((int16_t)rxData[5] << 8) | rxData[4] ;;
//
/// no biases applied just scale factor
_ax = (float)_axcounts * _accelScale;
_ay = (float)_aycounts * _accelScale;
_az = (float)_azcounts * _accelScale;
//
_gx = (float)_gxcounts * _gyroScale;
_gy = (float)_gycounts * _gyroScale;
_gz = (float)_gzcounts * _gyroScale;
//
_hx = (float)_hxcounts * _magScaleX;
_hy = (float)_hycounts * _magScaleY;
_hz = (float)_hzcounts * _magScaleZ;
////
log_i ( "ax = %f", ( _ax * 1000.0f) );
log_i ( " ay = %f", ( _ay * 1000.0f) );
log_i ( " az = %f", ( _az * 1000.0f ) );
log_i ( " gx = %f", ( _gx * 1000.0f ) );
log_i ( " gy = %f", ( _gy * 1000.0f ) );
log_i ( " gz = %f", ( _gz * 1000.0f ) );
log_i ( " hx = %f", _hx );
log_i ( " hy = %f", _hy );
log_i ( " hz = %f", _hz );
// h value multiplied by 10 converts to mG from uT
MahonyQuaternionUpdate ( _ax, _ay, _az, _gx, _gy, _gz, _hx * 10.0f, _hy * 10.0f, _hz * 10.0f );
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
float yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
float pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
float roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 13.13f; // Declination
roll *= 180.0f / PI;
TimePast = TimeNow;
// log_i ( " Roll: %f", roll );
// log_i ( " Pitch: %f", pitch );
// log_i ( " Yaw: %f", yaw );
}
else
{
log_i ( "WHO AM I FAILED!! ");
log_i ( "MPU9250 OK = %d", MPU9250_OK );
log_i ( " AK8963 OK = %d", AK8963_OK );
}
// xLastWakeTime = xTaskGetTickCount();
}
vTaskDelete(NULL);
} // void fGetIMU( void *pvParameters )