/*********************************************************************
* @fn readGyroData
*
* @brief Read gyroscope data
*
* @param none
*
* @return none
*/
static void readGyroData( void )
{
uint8 gData[GYROSCOPE_DATA_LEN];
if (HalGyroRead(gData))
{
Gyro_SetParameter( SENSOR_DATA, GYROSCOPE_DATA_LEN, gData);
}
}
/*********************************************************************
* @fn readGyroData
*
* @brief Read gyroscope data
*
* @param none
*
* @return none
*/
static void readGyroData( void )
{
uint8 aData[ACCELEROMETER_DATA_LEN];
uint8 gData[GYROSCOPE_DATA_LEN];
//GAPRole_SendUpdateParam( DEFAULT_DESIRED_MIN_CONN_INTERVAL, DEFAULT_DESIRED_MAX_CONN_INTERVAL, DEFAULT_DESIRED_CONN_TIMEOUT, DEFAULT_DESIRED_CONN_TIMEOUT, GAPROLE_TERMINATE_LINK);
if (HalGyroRead(aData,gData))
{
Accel_SetParameter( ACCELEROMETER_DATA, ACCELEROMETER_DATA_LEN, aData);
Gyro_SetParameter( GYROSCOPE_DATA, GYROSCOPE_DATA_LEN, gData);
}
}
/**************************************************************************************************
* @fn HalMotionHandleMeasurementStartEvent
*
* @brief Handles event indicating a new 10 msec measurement cycle has begun,
* so we should start taking samples.
*
* @param None
*
* @return None
**************************************************************************************************/
static void HalMotionHandleMeasurementStartEvent( void )
{
if (HalMotionState == HAL_MOTION_STATE_WAITING_FOR_NEXT_MEASUREMENT)
{
int16 x, y, z;
#if (HAL_MOTION_ENABLE_ROLL_COMPENSATION == TRUE)
int8 ax, ay, az;
#endif
/* Update state */
HalMotionState = HAL_MOTION_STATE_MEASURING;
HalGyroRead(&x, &y, &z );
samples.GyroSamples.X = x;
samples.GyroSamples.Y = -y;
samples.GyroSamples.Z = z;
/* Done with Gyro samples, now get Accelerometer samples */
#if (HAL_MOTION_ENABLE_ROLL_COMPENSATION == TRUE)
HalGyroReadAccelData( &ax, &ay, &az );
samples.AccSamples.X = ax;
samples.AccSamples.Y = ay;
samples.AccSamples.Z = -az;
#endif
/* Done with samples, so call Movea library routine to process them */
motion_status = AIR_MOTION_ProcessDelta(samples);
/* Update state */
HalMotionState = HAL_MOTION_STATE_WAITING_FOR_NEXT_MEASUREMENT;
if (motion_status.Status.IsDeltaComputed)
{
/* Avalid delta was computed */
/* Inform application that results are ready */
if (pHalMotionProcessFunction != NULL)
{
pHalMotionProcessFunction( motion_status.Delta.X,
motion_status.Delta.Y );
}
}
}
else if (HalMotionCalibrateGyro == TRUE && HalMotionState == HAL_MOTION_STATE_POWERING_ON)
{
// Currently calibrating
int16 x, y, z;
#if (HAL_MOTION_ENABLE_ROLL_COMPENSATION == TRUE)
int8 ax, ay, az;
#endif
HalMotionState = HAL_MOTION_STATE_MEASURING;
HalGyroRead( &x, &y, &z );
samples.GyroSamples.X = x;
samples.GyroSamples.Y = -y;
samples.GyroSamples.Z = z;
/* Done with Gyro samples, now get Accelerometer samples */
#if (HAL_MOTION_ENABLE_ROLL_COMPENSATION == TRUE)
HalGyroReadAccelData( &ax, &ay, &az );
samples.AccSamples.X = ax;
samples.AccSamples.Y = ay;
samples.AccSamples.Z = -az;
#endif
/* Done with samples, so call Movea library routine to process them */
motion_status = AIR_MOTION_ProcessDelta(samples);
if (motion_status.Status.NewGyroOffset == true)
{
// Found a new valid gyro offset
HalMotionGyroOffsets.x = motion_status.GyroOffsets.X;
HalMotionGyroOffsets.y = motion_status.GyroOffsets.Y;
HalMotionGyroOffsets.z = motion_status.GyroOffsets.Z;
/* Store gyro offsets in NVM */
(void) osal_snv_write( HAL_MOTION_NV_ITEM_GYRO_OFFSETS_ID,
sizeof( HalMotionGyroOffsets ),
(uint8 *)&HalMotionGyroOffsets );
/* Power down motion hardware and go back to initial state */
HalMotionDisable();
/* Inform application that calibration is done */
if (pHalMotionCalCompleteNotificationFunction != NULL)
{
pHalMotionCalCompleteNotificationFunction();
}
}
else if (HalNbSamplesCalCnt == 0)
{
// Too many attempts ==> Reset the cal process:
if (pHalMotionCalCompleteNotificationFunction != NULL)
{
/* Inform application that the current calibration is still valid */
pHalMotionCalCompleteNotificationFunction();
}
HalMotionDisable();
}
else
{
HalNbSamplesCalCnt--;
/* Update state */
HalMotionState = HAL_MOTION_STATE_POWERING_ON;
}
}
}