/*********************************************************************
* @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 HandleSensorData
* Handles sensor input data and feeds it to the sensor algorithms
*
***********************************************************************/
static void HandleSensorData(MessageBuffer *pRcvMsg)
{
osp_status_t status;
OSP_InputSensorData_t sensorData;
switch(pRcvMsg->msgId) {
case MSG_ACC_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgAccelData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgAccelData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgAccelData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgAccelData.timeStamp;
status = OSP_SetInputData(_AccHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
case MSG_MAG_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgMagData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgMagData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgMagData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgMagData.timeStamp;
status = OSP_SetInputData(_MagHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
case MSG_GYRO_DATA:
sensorData.data.data[0] = pRcvMsg->msg.msgGyroData.X;
sensorData.data.data[1] = pRcvMsg->msg.msgGyroData.Y;
sensorData.data.data[2] = pRcvMsg->msg.msgGyroData.Z;
sensorData.data.TimeStamp = pRcvMsg->msg.msgGyroData.timeStamp;
status = OSP_SetInputData(_GyroHandle, &sensorData);
ASF_assert(status == OSP_STATUS_OK);
break;
default:
D1_printf("ALG: Bad message: %d\r\n", pRcvMsg->msgId);
break;
}
}
// Enable GPIO clock and return GPIO base address
uint32_t Set_GPIO_Clock(uint32_t port_idx) {
#if 0
uint32_t gpio_add = 0;
switch (port_idx) {
case PortA:
gpio_add = GPIOA_BASE;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
break;
case PortB:
gpio_add = GPIOB_BASE;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
break;
case PortC:
gpio_add = GPIOC_BASE;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
break;
case PortD:
gpio_add = GPIOD_BASE;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
break;
default:
D1_printf("Port number is not correct.");
break;
}
return gpio_add;
#endif
return 0;
}
/*********************************************************************
* @fn HandleTimers
* Handles the MSG_TIMER_EXPIRY messages
*
*********************************************************************/
static void HandleTimers(MsgTimerExpiry *pTimerExp)
{
uint32_t timeStamp;
switch (pTimerExp->userValue) {
case TIMER_REF_PRESSURE_READ:
/* Schedule the next sampling */
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_PRESSURE_READ,
PRESSURE_SAMPLE_PERIOD, &sPressureTimer);
timeStamp = GetCurrentTime();
SensorDataHandler(PRESSURE_INPUT_SENSOR, timeStamp);
break;
#ifndef INTERRUPT_BASED_SAMPLING
case TIMER_REF_SENSOR_READ:
/* Schedule the next sampling */
ASFTimerStart(SENSOR_ACQ_TASK_ID, TIMER_REF_SENSOR_READ,
SENSOR_SAMPLE_PERIOD, &sSensorTimer);
/* Call data handler for each sensors */
timeStamp = RTC_GetCounter();
SensorDataHandler(ACCEL_INPUT_SENSOR, timeStamp);
SensorDataHandler(MAG_INPUT_SENSOR, timeStamp);
SensorDataHandler(GYRO_INPUT_SENSOR, timeStamp);
break;
#endif
default:
D1_printf("Unknown Timer! Reference %d\r\n", pTimerExp->userValue);
break;
}
}
/****************************************************************************************************
* @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;
}
}
}
/**
* Configure pin pull-up/pull-down
*/
void pin_mode(PinName pin, PinMode mode) {
#if 0
ASF_assert(pin != (PinName)NC);
GPIO_InitTypeDef GPIO_InitStructure;
uint32_t port_index = STM_PORT(pin);
uint32_t pin_index = STM_PIN(pin);
// Enable GPIO clock
uint32_t gpio_add = Set_GPIO_Clock(port_index);
GPIO_TypeDef *gpio = (GPIO_TypeDef *)gpio_add;
// Configure open-drain and pull-up/down
switch (mode) {
case AnalogInput:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
break;
case Floating:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
break;
case PullUp:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
break;
case PullDown:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
break;
case OpenDrain:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
break;
case PushPullOutput:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
break;
case OpenDrainOutputAF:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
break;
case PushPullOutputAF:
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
break;
default:
D1_printf("PINMAP: Bad GPIO Mode: %d\r\n", mode);
break;
}
// Configure GPIO
GPIO_InitStructure.GPIO_Pin = (uint16_t)pin_index;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(gpio, &GPIO_InitStructure);
#endif
}
/****************************************************************************************************
* @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 i2cslave_start
* Slave I2C Start callback function
*
***************************************************************************************************/
static void i2cslave_start(uint8_t addr)
{
/* Do Nothing */
//D0_printf("Slave Addr %x\n", addr);
i2c_t *obj = pi2c_slave_obj; /* get the record*/
if( obj->i2c_operation == I2C_READ_IN_PROGRESS)
{
D1_printf(" Rep St \r\n");
obj->i2c_operation = I2C_READ_COMPLETE;
}
else
{
obj->i2c_operation = I2C_IDLE;
obj->pXfer.rxBuff = obj->rxBuff;
obj->pXfer.rxSz = 0;
obj->pXfer.status = ERR_I2C_BUSY;
obj->pXfer.bytesSent = 0;
obj->pXfer.bytesRecv = 0;
}
}
void gpio_irq_enable(gpio_irq_t *obj) {
switch(obj->event){
case IRQ_EDGE_RISE:
Chip_PININT_SetPinModeEdge(LPC_PININT, obj->irq_index); /* edge sensitive */
Chip_PININT_EnableIntHigh(LPC_PININT, obj->irq_index); /* Rising edge interrupt */
break;
case IRQ_EDGE_FALL:
Chip_PININT_SetPinModeEdge(LPC_PININT, obj->irq_index); /* Edge sensitive */
Chip_PININT_EnableIntLow(LPC_PININT, obj->irq_index); /* Falling edge interrupt */
break;
case IRQ_LEVEL_HIGH:
Chip_PININT_SetPinModeLevel(LPC_PININT, obj->irq_index); /* Level sensitive */
Chip_PININT_EnableIntHigh(LPC_PININT, obj->irq_index); /* High level interrupt */
break;
case IRQ_LEVEL_LOW:
Chip_PININT_SetPinModeLevel(LPC_PININT, obj->irq_index); /* Level sensitive */
Chip_PININT_EnableIntLow(LPC_PININT, obj->irq_index); /* Low level interrupt */
break;
default:
D1_printf("GPIO_IRQ_ENABLE: Bad IRQ Mode: %d\r\n", obj->irq_index);
while(1){};
}
}
/*********************************************************************
* @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;
}
}
}
