/****************************************************************************************************
* @fn I2CCommTask
* This tasks primary goal is to serialize the communication request (sensor results) going
* over I2C
*
* @param none
*
* @return none
*
***************************************************************************************************/
ASF_TASK void I2CCommTask( ASF_TASK_ARG )
{
MessageBuffer *rcvMsg = NULLP;
/* I2C Slave mode initialization */
I2C_Slave_init();
/* Init register area for slave */
SH_Slave_init();
while(1)
{
ASFReceiveMessage(I2CSLAVE_COMM_TASK_ID, &rcvMsg );
switch (rcvMsg->msgId)
{
case MSG_ACC_DATA:
SendSensorData(AP_PSENSOR_ACCELEROMETER_RAW, &rcvMsg->msg.msgAccelData);
break;
case MSG_MAG_DATA:
SendSensorData(AP_PSENSOR_MAGNETIC_FIELD_RAW, &rcvMsg->msg.msgMagData);
break;
case MSG_GYRO_DATA:
SendSensorData(AP_PSENSOR_GYROSCOPE_RAW, &rcvMsg->msg.msgGyroData);
break;
default:
D1_printf("I2C:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
/*********************************************************************
* @fn AlgBackGndTask
* This task is responsible for running the background
* routines (e.g. calibration)
*
* @param none
*
* @return none
*
*********************************************************************/
ASF_TASK void AlgBackGndTask (ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
D0_printf(__func__);
D0_printf("\r\n");
while (1) {
ASFReceiveMessage(ALG_BG_TASK_ID, &rcvMsg);
switch (rcvMsg->msgId) {
case MSG_TRIG_ALG_BG:
/*
* background compute.
* Note that it's safe to call background processing
* more often than needed
*/
/* Bump clock speed while processing? HY-DBG */
while(OSP_DoBackgroundProcessing() != OSP_STATUS_IDLE);
break;
default:
/* Unhandled messages */
D1_printf("Alg-BG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( ALG_BG_TASK_ID, &rcvMsg );
}
}
/****************************************************************************************************
* @fn InstrManagerTask
* This task is for instrumentation. At startup initializes
* other tasks and OS resources in the system. This task is created by the main OS entry
* function called from main().
*
* @param none
*
* @return none
*
***************************************************************************************************/
ASF_TASK void InstrManagerTask( ASF_TASK_ARG )
{
MessageBuffer *rcvMsg = NULLP;
osp_bool_t msgHandled;
/* Create other tasks & OS resources in the system */
InitializeTasks();
/* Call user init related to instrumentation (for stuff needing to be done before the while loop) */
InstrManagerUserInit();
while(1)
{
/* User instrumentation such as LED indications, RTC updates, etc. */
ASFReceiveMessage( INSTR_MANAGER_TASK_ID, &rcvMsg );
msgHandled = InstrManagerUserHandler( rcvMsg );
if (!msgHandled)
{
switch (rcvMsg->msgId)
{
#if defined ON_DEMAND_PROFILING && defined ASF_PROFILING
case MSG_PROFILING_REQ:
//DoProfiling(true); /* Enable this if Stack addresses are needed */
DoProfiling(false);
break;
#endif
case MSG_TIMER_EXPIRY:
break;
default:
/* Unhandled messages */
D1_printf("INSTR:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
}
/****************************************************************************************************
* @fn AlgBackGndTask
* This task is responsible for running the background routines (e.g. calibration)
*
* @param none
*
* @return none
*
***************************************************************************************************/
ASF_TASK void AlgBackGndTask ( ASF_TASK_ARG )
{
MessageBuffer *rcvMsg = NULLP;
while (1)
{
ASFReceiveMessage( ALG_BG_TASK_ID, &rcvMsg );
switch (rcvMsg->msgId)
{
case MSG_TRIG_ALG_BG:
while(OSP_DoBackgroundProcessing() != OSP_STATUS_IDLE)
; //background compute. Note that it's safe to call background processing more often than needed
break;
default:
/* Unhandled messages */
D1_printf("Alg-BG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
/*********************************************************************
* @fn AlgorithmTask
* This task is responsible for running the sensor algorithms
* on the incoming sensor data (could be raw or filtered) and
* processing output results
*
* @param none
*
* @return none
*
**********************************************************************/
ASF_TASK void AlgorithmTask (ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
OSP_STATUS_t OSP_Status;
int alg_count;
OSP_GetLibraryVersion(&version);
D1_printf("OSP Version: %s\r\n", version->VersionString);
/* Initialize the mutex */
mutex_id = osMutexCreate(osMutex(mutexCritSection));
OSP_Status = OSP_Initialize(&gSystemDesc);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_Initialize Failed");
OSP_SetCalibrationConfig( 0x1); // disable rotational cal.
D0_printf("--Alg Task %i\r\n", __LINE__);
// Register the input sensors
OSP_Status = OSP_RegisterInputSensor(&_AccSensDesc, &_AccHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (accel) Failed");
OSP_Status = OSP_RegisterInputSensor(&_MagSensDesc, &_MagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (mag) Failed");
OSP_Status = OSP_RegisterInputSensor(&_GyroSensDesc, &_GyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "OSP_RegisterInputSensor (gyro) Failed");
#if 0
SENSOR_SUBSCRIBE(SENSOR_STEP_COUNTER);
SENSOR_SUBSCRIBE(SENSOR_STEP_DETECTOR);
SENSOR_SUBSCRIBE(SENSOR_SIGNIFICANT_MOTION);
SENSOR_SUBSCRIBE(SENSOR_GYROSCOPE_UNCALIBRATED);
SENSOR_SUBSCRIBE(SENSOR_MAGNETIC_FIELD_UNCALIBRATED);
SENSOR_SUBSCRIBE(SENSOR_GYROSCOPE);
SENSOR_SUBSCRIBE(SENSOR_ACCELEROMETER);
SENSOR_SUBSCRIBE(SENSOR_MAGNETIC_FIELD);
SENSOR_SUBSCRIBE(SENSOR_ORIENTATION);
SENSOR_SUBSCRIBE(SENSOR_GRAVITY);
SENSOR_SUBSCRIBE(SENSOR_LINEAR_ACCELERATION);
SENSOR_SUBSCRIBE(SENSOR_ROTATION_VECTOR);
SENSOR_SUBSCRIBE(SENSOR_GAME_ROTATION_VECTOR);
SENSOR_SUBSCRIBE(SENSOR_GEOMAGNETIC_ROTATION_VECTOR);
// Subscribing private sensor results
PRIVATE_SENSOR_SUBSCRIBE(AP_PSENSOR_ACCELEROMETER_UNCALIBRATED);
#endif
D0_printf("%s: --Alg Task init done\r\n", __func__);
while (1) {
ASFReceiveMessage(ALGORITHM_TASK_ID, &rcvMsg);
if (!(mycount % 64)) {
LED_Toggle(LED_GREEN);
}
switch (rcvMsg->msgId) {
case MSG_MAG_DATA:
// SendBgTrigger();
case MSG_ACC_DATA:
case MSG_GYRO_DATA:
mycount++;
HandleSensorData(rcvMsg);
//keep doing foreground computation until its finished
/* Bump clock speed while processing? HY-DBG */
alg_count = 0;
do {
OSP_Status = OSP_DoForegroundProcessing();
ASF_assert(OSP_Status != OSP_STATUS_UNSPECIFIED_ERROR);
alg_count++;
if (alg_count > 5) {
D0_printf("%s:%i Taking too long\r\n", __func__, __LINE__);
break;
}
} while(OSP_Status != OSP_STATUS_IDLE);
/* DBG:
* Run background here as the backgound taks doesn't seem to run enough */
while(OSP_DoBackgroundProcessing() != OSP_STATUS_IDLE);
break;
case MSG_PRESS_DATA:
PressureDataResultCallback(&rcvMsg->msg.msgPressData);
break;
default:
/* Unhandled messages */
D1_printf("Alg-FG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( ALGORITHM_TASK_ID, &rcvMsg );
#ifdef DEBUG_TEST_SENSOR_SUBSCRIPTION
// Testing subscribe and unsubscribe sensors
DebugTestSensorSubscription();
#endif
}
}
/****************************************************************************************************
* @fn AlgorithmTask
* This task is responsible for running the sensor algorithms on the incoming sensor
* data (could be raw or filtered) and processing output results
*
* @param none
*
* @return none
*
***************************************************************************************************/
ASF_TASK void AlgorithmTask ( ASF_TASK_ARG )
{
MessageBuffer *rcvMsg = NULLP;
osp_status_t OSP_Status;
OSP_GetVersion(&version);
D1_printf("OSP Version: %s\r\n", version->VersionString);
OSP_Status = OSP_Initialize(&gSystemDesc);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_Initialize Failed");
// Register the input sensors
OSP_Status = OSP_RegisterInputSensor(&_AccSensDesc, &_AccHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (accel) Failed");
OSP_Status = OSP_RegisterInputSensor(&_MagSensDesc, &_MagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (mag) Failed");
OSP_Status = OSP_RegisterInputSensor(&_GyroSensDesc, &_GyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_RegisterSensor (gyro) Failed");
// Register output sensors/results
OSP_Status = OSP_SubscribeOutputSensor(&stepCounterRequest, &_stepCounterHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_STEP_COUNTER) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&sigMotionRequest, &_sigMotionHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_CONTEXT_DEVICE_MOTION) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalAccelRequest, &_unCalAccelHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_ACCELEROMETER) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalMagRequest, &_unCalMagHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_MAGNETIC_FIELD) Failed");
OSP_Status = OSP_SubscribeOutputSensor(&UnCalGyroRequest, &_unCalGyroHandle);
ASF_assert_msg(OSP_STATUS_OK == OSP_Status, "SensorManager: OSP_SubscribeResult (SENSOR_GYROSCOPE) Failed");
while (1)
{
ASFReceiveMessage( ALGORITHM_TASK_ID, &rcvMsg );
switch (rcvMsg->msgId)
{
case MSG_MAG_DATA:
SendBgTrigger();
case MSG_ACC_DATA:
case MSG_GYRO_DATA:
HandleSensorData(rcvMsg);
do
{
OSP_Status = OSP_DoForegroundProcessing();
ASF_assert(OSP_Status != OSP_STATUS_ERROR);
} while(OSP_Status != OSP_STATUS_IDLE)
; //keep doing foreground computation until its finished
break;
default:
/* Unhandled messages */
D1_printf("Alg-FG:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
/* Sensor data flow:
* 1. Sensor interrupts on data ready.
* 2. IRQ handler is solely expected to call SendDataReadyIndication.
* 3. SensorAcqTask will see a message data is available.
* 4. Data is read. Sensor driver is expected to clear
* the interrupt from the read function.
*/
ASF_TASK void SensorAcqTask(ASF_TASK_ARG)
{
MessageBuffer *rcvMsg = NULLP;
volatile uint8_t i;
#ifndef WAIT_FOR_HOST_SYNC
osDelay(50);
#else
WaitForHostSync(); //This also allows for startup time for sensors
#endif
/* Setup I2C bus here? */
dev_i2c_init();
/* Setup interface for the Magnetometer */
Mag_HardwareSetup(true);
Mag_Initialize();
/* Setup interface for the accelerometers */
Accel_HardwareSetup(true);
Accel_Initialize(INIT_NORMAL);
/* Setup Gyro */
Gyro_HardwareSetup(true);
Gyro_Initialize();
D0_printf("Gyro init done:\r\n");
/* Setup Pressure */
Pressure_HardwareSetup(true);
Pressure_Initialize();
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_PRESSURE_READ,
PRESSURE_SAMPLE_PERIOD, &sPressureTimer);
#ifndef INTERRUPT_BASED_SAMPLING
/* Start sample period timer */
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_SENSOR_READ,
SENSOR_SAMPLE_PERIOD, &sSensorTimer);
#else
/* Enable Sensor interrupts */
Mag_ConfigDataInt(true);
Accel_ConfigDataInt(true);
Gyro_ConfigDataInt(true);
# ifdef TRIGGERED_MAG_SAMPLING
D0_printf("Set mag to low power mode\r\n");
Mag_SetLowPowerMode(); //Low power mode until triggered
# endif
#endif
D0_printf("%s initialized\r\n", __func__);
/* Magnetometer sensor does not re-generate interrupt if its outputs are not read. */
Mag_ClearDataInt();
/* Indicate sensor init done */
sTskRdyFlag = 1;
while (1) {
ASFReceiveMessage(SENSOR_ACQ_TASK_ID, &rcvMsg);
// D0_printf("rcvMsg->msgId = %d\r\n",rcvMsg->msgId);
switch (rcvMsg->msgId) {
case MSG_TIMER_EXPIRY:
HandleTimers(&rcvMsg->msg.msgTimerExpiry);
break;
case MSG_CAL_EVT_NOTIFY:
#ifdef ENABLE_FLASH_STORE
StoreCalibrationData((CalEvent_t)rcvMsg->msg.msgCalEvtNotify.byte);
#else
D0_printf("#### WARNING - NV Storage Not Implemented! #####\r\n");
#endif
break;
case MSG_SENSOR_DATA_RDY:
//D0_printf("MSG_SENSOR_DATA_RDY msg id %d\r\n", rcvMsg->msgId);
#ifdef INTERRUPT_BASED_SAMPLING
SensorDataHandler((InputSensor_t)rcvMsg->msg.msgSensorDataRdy.sensorId,
rcvMsg->msg.msgSensorDataRdy.timeStamp);
#endif
break;
case MSG_SENSOR_CONTROL:
//D0_printf("MSG_SENSOR_CONTROL msg id %d\r\n",rcvMsg->msgId);
SensorControlCmdHandler(&rcvMsg->msg.msgSensorControlData);
break;
default:
/* Unhandled messages */
D2_printf("SensorAcqTask:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
ASFDeleteMessage( SENSOR_ACQ_TASK_ID, &rcvMsg );
}
}
