/**************************************************************************************************** * @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 ); } }