/****************************************************************************************************
* @fn SendBgTrigger
* Sends triggers to the algorithm background task to do processing
*
***************************************************************************************************/
static void SendBgTrigger( void )
{
MessageBuffer *pSendMsg = NULLP;
ASF_assert( ASFCreateMessage( MSG_TRIG_ALG_BG, sizeof(MsgNoData), &pSendMsg ) == ASF_OK );
ASFSendMessage( ALG_BG_TASK_ID, pSendMsg );
}
static void SensorDataHandler(InputSensor_t sensorId, uint32_t timeStamp)
{
MsgAccelData accelData;
MsgMagData magData;
MsgGyroData gyroData;
MsgPressData pressData;
static uint8_t sMagDecimateCount = 0;
static uint8_t gyroSampleCount = 0;
static uint8_t accSampleCount = 0;
#if defined ALGORITHM_TASK
MessageBuffer *pMagSample = NULLP;
MessageBuffer *pAccSample = NULLP;
MessageBuffer *pGyroSample = NULLP;
MessageBuffer *pPressSample = NULLP;
#endif
switch (sensorId) {
case MAG_INPUT_SENSOR:
/* Read mag Data - reading would clear interrupt also */
Mag_ReadData(&magData);
if ((sMagDecimateCount++ % MAG_DECIMATE_FACTOR) == 0) {
/* Replace time stamp with that captured
by interrupt handler */
magData.timeStamp = timeStamp;
#ifdef ALGORITHM_TASK
ASF_assert(ASFCreateMessage(MSG_MAG_DATA,
sizeof(MsgMagData),
&pMagSample) == ASF_OK);
pMagSample->msg.msgMagData = magData;
if (!(ASFSendMessage(ALGORITHM_TASK_ID,
pMagSample) == ASF_OK)) {
queue_err++;
}
#endif
}
//D0_printf("Mag ADC: %d, %d, %d\r\n", magData.X, magData.Y, magData.Z);
break;
case GYRO_INPUT_SENSOR:
Gyro_ReadData(&gyroData); //Reads also clears DRDY interrupt
/* Replace time stamp with that captured by interrupt handler */
if ((gyroSampleCount++ % GYRO_SAMPLE_DECIMATE) == 0) {
/* Read Gyro Data - reading typically clears interrupt as well */
gyroData.timeStamp = timeStamp;
#ifdef ALGORITHM_TASK
if (ASFCreateMessage(MSG_GYRO_DATA,
sizeof(MsgGyroData),
&pGyroSample) == ASF_OK) {
pGyroSample->msg.msgGyroData = gyroData;
if (!(ASFSendMessage(ALGORITHM_TASK_ID,
pGyroSample) == ASF_OK)) {
queue_err++;
}
}
#endif
// D0_printf("Gyro ADC: %d, %d, %d\r\n", gyroData.X, gyroData.Y, gyroData.Z);
}
break;
case ACCEL_INPUT_SENSOR:
#if defined TRIGGERED_MAG_SAMPLING
if (accSampleCount % MAG_TRIGGER_RATE_DECIMATE == 0) {
Mag_TriggerDataAcq(); //Mag is triggered relative to Accel to avoid running separate timer
}
#endif
/* Read Accel Data - reading typically clears interrupt as well */
Accel_ReadData(&accelData);
if (accSampleCount++ % ACCEL_SAMPLE_DECIMATE == 0) {
/* Replace time stamp with that captured by interrupt handler */
accelData.timeStamp = timeStamp;
#ifdef ALGORITHM_TASK
if (ASFCreateMessage(MSG_ACC_DATA, sizeof(MsgAccelData),
&pAccSample) == ASF_OK) {
pAccSample->msg.msgAccelData = accelData;
if (!(ASFSendMessage(ALGORITHM_TASK_ID,
pAccSample) == ASF_OK)) {
queue_err++;
}
}
#endif
// D0_printf("Accel ADC: %d, %d, %d\r\n", accelData.X, accelData.Y, accelData.Z);
}
break;
case PRESSURE_INPUT_SENSOR:
/* Read Accel Data - reading typically clears interrupt as well */
Pressure_ReadData(&pressData);
/* Replace time stamp with that captured by interrupt handler */
pressData.timeStamp = timeStamp;
/* To do: Send the pressure sensor out */
#ifdef ALGORITHM_TASK
ASF_assert(ASFCreateMessage(MSG_PRESS_DATA,
sizeof(MsgPressData),
&pPressSample) == ASF_OK);
pPressSample->msg.msgPressData = pressData;
ASFSendMessage(ALGORITHM_TASK_ID, pPressSample);
#endif
// D0_printf("Pressure ADC: P = %d, T = %d\r\n", pressData.X, pressData.Z);
break;
default:
D0_printf("Input Sensor ID %d is not supported\r\n", sensorId);
break;
}
}
