/******************************************************************************
*
* @name Send_Task
*
* @brief
*
* @param None
*
* @return None
*****************************************************************************/
void Send_Task(uint_32 param)
{
while(TRUE)
{
if(g_bridge.usb_enum_complete == FALSE)
{
continue;
}
if(_lwevent_wait_for(&lwevent,BRIDGE_SEND_EVENT,FALSE,NULL) !=
MQX_OK)
{ /* since third argument is false, the execution continues when any
of the evnt is generated */
#if _DEBUG
printf("Event Wait failed : Send_Task\n");
#endif
_task_block();
}
/* clear the event and proceed*/
if(_lwevent_clear(&lwevent,BRIDGE_SEND_EVENT) != MQX_OK)
{
#if _DEBUG
printf("\nSend Event Clear failed");
#endif
_task_block();
}
if(g_connection_present == TRUE)
{
Bridge_Interface_Write(g_tx_buff_ptr, g_bridge_tx_size);
}
}
}
/******************************************************************************
*
* @name Recv_Task
*
* @brief
*
* @param None
*
* @return None
*****************************************************************************/
void Recv_Task(uint_32 param)
{
while(TRUE)
{
if(g_bridge.usb_enum_complete == FALSE)
{
continue;
}
if(_lwevent_wait_for(&lwevent,BRIDGE_RECV_EVENT,FALSE,NULL) !=
MQX_OK)
{ /* since third argument is false, the execution continues when any
of the evnt is generated */
#if _DEBUG
printf("Event Wait failed : Recv_Task\n");
#endif
_task_block();
}
/* clear the event and proceed*/
if(_lwevent_clear(&lwevent,BRIDGE_RECV_EVENT) != MQX_OK)
{
#if _DEBUG
printf("\nRecv Event Clear failed");
#endif
_task_block();
}
if(g_connection_present == TRUE)
{
/* Queue recv on Bridge Interface of max buffer size*/
Bridge_Interface_Read(g_bridge_rx_buff_ptr, PHDC_BULK_IN_EP_SIZE);
}
}
}
// magnetic calibration task
void MagCal_task(uint32_t task_init_data)
{
while(1)
{
// set the RED LED off and set test pin off ready for magnetic calibration to run
LED_RED_SetVal(NULL);
TestPin_MagCal_Time_ClrVal(NULL);
// wait for the magnetic calibration event
// this event will never be enabled for build options which don't require magnetic calibration
// FALSE means any bit (of the 1 bit enabled by the mask) unblocks
// and NULL means timeout is infinite
_lwevent_wait_for(&(mqxglobals.MagCalEventStruct), 1, FALSE, NULL);
// set the red LED on and test pin pin on
LED_RED_ClrVal(NULL);
TestPin_MagCal_Time_SetVal(NULL);
// prevent compilation errors when magnetic calibration is not required
#if defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
// and run the magnetic calibration
MagCal_Run(&thisMagCal, &thisMagBuffer);
#endif
} // end of infinite loop
return;
}
// sensor read task
void RdSensData_task(uint32_t task_init_data)
{
// initialize the physical sensors over I2C and the sensor data structures
RdSensData_Init();
// initialize the user high frequency (typically 200Hz) task
UserHighFrequencyTaskInit();
// loop indefinitely (typically 200Hz)
while(1)
{
// wait here for the sampling event (hardware clock, typically at 200Hz)
// the Kalman filter and magnetic fusion tasks execute while this task is blocked here
// FALSE means any bit (of the 1 bit enabled by the mask) unblocks
// and NULL means timeout is infinite
_lwevent_wait_for(&(mqxglobals.SamplingEventStruct), 1, FALSE, NULL);
// reset the Kalman filter task flag
mqxglobals.RunKF_Event_Flag = 0;
// read the sensors
RdSensData_Run();
// run the user high frequency task
UserHighFrequencyTaskRun();
// use the Kalman filter flag set by the sensor read task (typically every 8 iterations)
// with a mask of 1 (least significant bit set) to enable the Kalman filter task to run
if (mqxglobals.RunKF_Event_Flag)
_lwevent_set(&(mqxglobals.RunKFEventStruct), 1);
} // end of infinite loop
}
// Kalman filter sensor fusion task
void Fusion_task(uint32_t task_init_data)
{
uint16_t LedGreenCounter = 0;
// initialize the sensor fusion algorithms
Fusion_Init();
// initialize the user medium frequency (typically 25Hz) task
UserMediumFrequencyTaskInit();
// infinite loop controlled by MQX-Lite events
while(1)
{
// ensure the red LED (power up check) is off (line high)
LED_RED_SetVal(NULL);
// set the output test pin to zero (for timing measurements)
TestPin_KF_Time_ClrVal(NULL);
// wait for the sensor fusion event to occur
// FALSE means any bit (of the 1 bit enabled by the mask) unblocks
// and NULL means timeout is infinite
_lwevent_wait_for(&(mqxglobals.RunKFEventStruct), 1, FALSE, NULL);
// set the output test pin to high to denote sensor fusion running
TestPin_KF_Time_SetVal(NULL);
// flash the green LED to denote the sensor fusion is running
if (++LedGreenCounter >= 5)
{
LED_GREEN_NegVal(NULL);
LedGreenCounter = 0;
}
// reset the magnetic calibration flag (this is set by the fusion algorithm)
mqxglobals.MagCal_Event_Flag = 0;
// call the sensor fusion algorithms
Fusion_Run();
// run the user medium frequency (typically 25Hz) user task
UserMediumFrequencyTaskRun();
// enable the magnetic calibration event if the flag for a new magnetic calibration
// with a mask of 1 (least significant bit set)
// was set by the sensor fusion algorithms
if (mqxglobals.MagCal_Event_Flag)
{
_lwevent_set(&(mqxglobals.MagCalEventStruct), 1);
}
} // end of infinite loop
return;
}
/*!
* \brief MQX API handler for usermode - part of wrapper around standard MQX API
* which require privilege mode.
*
* \param[in] api_no API number - number of wrapped function
* \param[in] params generic parameter - direct use with called MQX API fn
*
* \return uint32_t return of called function
*/
uint32_t _mqx_api_call_handler
(
// [IN] API number - number of wrapped function
MQX_API_NUMBER_ENUM api_no,
// [IN] generic parameter - direct use with called MQX API fn
MQX_API_CALL_PARAMS_PTR params
)
{
int32_t res = -1;
uint32_t param0 = params->param0;
uint32_t param1 = params->param1;
uint32_t param2 = params->param2;
uint32_t param3 = params->param3;
uint32_t param4 = params->param4;
switch (api_no) {
// _lwsem
case MQX_API_LWSEM_POLL:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)))
res = (uint32_t)_lwsem_poll((LWSEM_STRUCT_PTR)param0);
break;
case MQX_API_LWSEM_POST:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)))
res = _lwsem_post((LWSEM_STRUCT_PTR)param0);
break;
case MQX_API_LWSEM_WAIT:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)))
res = _lwsem_wait((LWSEM_STRUCT_PTR)param0);
break;
case MQX_API_LWSEM_CREATE:
res = _lwsem_create_internal((LWSEM_STRUCT_PTR)param0, (_mqx_int)param1, (bool)param2, TRUE);
break;
#if MQX_HAS_TICK
case MQX_API_LWSEM_WAIT_FOR:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)) && (!param1 || _psp_mem_check_access(param1, sizeof(MQX_TICK_STRUCT), MPU_UM_RW)))
res = _lwsem_wait_for((LWSEM_STRUCT_PTR)param0, (MQX_TICK_STRUCT_PTR)param1);
break;
case MQX_API_LWSEM_WAIT_TICKS:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)))
res = _lwsem_wait_ticks((LWSEM_STRUCT_PTR)param0, (_mqx_uint)param1);
break;
case MQX_API_LWSEM_WAIT_UNTIL:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0)) && (!param1 || _psp_mem_check_access(param1, sizeof(MQX_TICK_STRUCT), MPU_UM_RW)))
res = _lwsem_wait_until((LWSEM_STRUCT_PTR)param0, (MQX_TICK_STRUCT_PTR)param1);
break;
case MQX_API_LWSEM_DESTROY:
if (MQX_OK == (res = _lwsem_usr_check((LWSEM_STRUCT_PTR)param0))) {
res = _lwsem_destroy_internal((LWSEM_STRUCT_PTR)param0, TRUE);
}
break;
#endif // MQX_HAS_TICK
// _lwevent
#if MQX_USE_LWEVENTS
case MQX_API_LWEVENT_CLEAR:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0)))
res = _lwevent_clear((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1);
break;
case MQX_API_LWEVENT_SET:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0)))
res = _lwevent_set((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1);
break;
case MQX_API_LWEVENT_SET_AUTO_CLEAR:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0)))
res = _lwevent_set_auto_clear((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1);
break;
case MQX_API_LWEVENT_WAIT_FOR:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0)) && \
(!param3 || _psp_mem_check_access(param3, sizeof(MQX_TICK_STRUCT), MPU_UM_RW))) {
res = _lwevent_wait_for((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1, (bool)param2, (MQX_TICK_STRUCT_PTR)param3);
}
break;
case MQX_API_LWEVENT_WAIT_FOR_TICKS:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0))) {
res = _lwevent_wait_ticks((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1, (bool)param2, (_mqx_uint)param3);
}
break;
case MQX_API_LWEVENT_WAIT_UNTIL:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0)) && \
(!param3 || _psp_mem_check_access(param3, sizeof(MQX_TICK_STRUCT), MPU_UM_RW))) {
res = _lwevent_wait_until((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1, (bool)param2, (MQX_TICK_STRUCT_PTR)param3);
}
break;
case MQX_API_LWEVENT_GET_SIGNALLED:
res = _lwevent_get_signalled();
break;
case MQX_API_LWEVENT_CREATE:
res = _lwevent_create_internal((LWEVENT_STRUCT_PTR)param0, (_mqx_uint)param1, TRUE);
break;
case MQX_API_LWEVENT_DESTROY:
if (MQX_OK == (res = _lwevent_usr_check((LWEVENT_STRUCT_PTR)param0))) {
res = _lwevent_destroy_internal((LWEVENT_STRUCT_PTR)param0, TRUE);
}
break;
#endif
#if MQX_USE_LWMSGQ
case MQX_API_LWMSGQ_INIT:
res = _lwmsgq_init_internal((void *)param0, (_mqx_uint)param1, (_mqx_uint)param2, TRUE);
break;
case MQX_API_LWMSGQ_RECEIVE:
if (MQX_OK == (res = _lwmsgq_usr_check((LWMSGQ_STRUCT_PTR)param0)) && \
_psp_mem_check_access(param1, ((LWMSGQ_STRUCT_PTR)param0)->MSG_SIZE, MPU_UM_RW) && \
(!param4 || _psp_mem_check_access(param4, sizeof(MQX_TICK_STRUCT), MPU_UM_RW)))
res = _lwmsgq_receive((void *)param0, (_mqx_max_type_ptr)param1, (_mqx_uint)param2, (_mqx_uint)param3, (MQX_TICK_STRUCT_PTR)param4);
break;
case MQX_API_LWMSGQ_SEND:
if (MQX_OK == (res = _lwmsgq_usr_check((LWMSGQ_STRUCT_PTR)param0)) && \
_psp_mem_check_access(param1, ((LWMSGQ_STRUCT_PTR)param0)->MSG_SIZE, MPU_UM_RW))
res = _lwmsgq_send((void *)param0, (_mqx_max_type_ptr)param1, (_mqx_uint)param2);
break;
#endif // MQX_USE_LWMSGQ
case MQX_API_TASK_CREATE:
res = _task_create_internal((_processor_number)param0, (_mqx_uint)param1, (uint32_t)param2, TRUE);
break;
case MQX_API_TASK_DESTROY:
res = _task_destroy_internal((_task_id)param0, TRUE);
break;
case MQX_API_TASK_ABORT:
res = _task_abort_internal((_task_id)param0, TRUE);
break;
case MQX_API_TASK_READY:
_task_ready((void *)param0);
res = MQX_OK; // irelevant, function is without return value
break;
case MQX_API_TASK_SET_ERROR:
res = _task_set_error((_mqx_uint)param0);
break;
case MQX_API_TASK_GET_TD:
res = (uint32_t)_task_get_td((_task_id)param0);
break;
#if MQX_USE_LWMEM
case MQX_API_LWMEM_ALLOC:
res = (uint32_t)_usr_lwmem_alloc_internal((_mem_size)param0);
break;
case MQX_API_LWMEM_ALLOC_FROM:
if (_psp_mem_check_access(param0, sizeof(LWMEM_POOL_STRUCT), MPU_UM_RW) && \
_psp_mem_check_access((uint32_t)(((LWMEM_POOL_STRUCT_PTR)param0)->POOL_ALLOC_START_PTR), (char*)(((LWMEM_POOL_STRUCT_PTR)param0)->POOL_ALLOC_END_PTR) - (char*)(((LWMEM_POOL_STRUCT_PTR)param0)->POOL_ALLOC_START_PTR), MPU_UM_RW))
res = (uint32_t)_lwmem_alloc_from((_lwmem_pool_id)param0, (_mem_size)param1);
else
res = 0; // NULL, allocation failed
break;
case MQX_API_LWMEM_FREE:
if (_psp_mem_check_access(param0, 4, MPU_UM_RW))
res = _lwmem_free((void *)param0);
break;
case MQX_API_LWMEM_CREATE_POOL:\
if (_psp_mem_check_access(param0, sizeof(LWMEM_POOL_STRUCT), MPU_UM_RW) && \
_psp_mem_check_access(param1, param2, MPU_UM_RW))
res = (uint32_t)_lwmem_create_pool((LWMEM_POOL_STRUCT_PTR)param0, (void *)param1, (_mem_size)param2);
break;
case MQX_API_LWMEM_REALLOC:
if (_psp_mem_check_access(param0, 4, MPU_UM_RW))
res = (uint32_t)_lwmem_realloc((void *)param0,(_mem_size)param1);
break;
#endif // MQX_USE_LWMEM
// _time
case MQX_API_TIME_DELAY:
_time_delay(param0);
res = MQX_OK; // irelevant, function is without return value
break;
#if MQX_HAS_TICK
case MQX_API_TIME_DELAY_TICKS:
_time_delay_ticks(param0);
res = MQX_OK; // irelevant, function is without return value
break;
case MQX_API_TIME_GET_ELAPSED_TICKS:
if (_psp_mem_check_access(param0, sizeof(MQX_TICK_STRUCT), MPU_UM_RW)) {
_time_get_elapsed_ticks((MQX_TICK_STRUCT_PTR)param0);
res = MQX_OK; // irelevant, function is without return value
}
else {
_task_set_error(MQX_ACCESS_ERROR);
}
break;
#endif // MQX_HAS_TICK
default:
while (1);
}
return res;
}
/*FUNCTION*----------------------------------------------------------------
*
* Function Name : Main_Task
* Returned Value : None
* Comments :
* First function called. This function is the entry for
* the PHDC Application
*
*END*--------------------------------------------------------------------*/
void Main_Task(uint_32 param)
{
/* Initialize Global Variable Structure */
PHDC_CONFIG_STRUCT phdc_config;
phdc_config.phdc_callback.callback = USB_App_Callback;
phdc_config.phdc_callback.arg = (void*)&g_bridge.handle;
phdc_config.vendor_callback.callback = NULL;
phdc_config.vendor_callback.arg = NULL;
phdc_config.desc_callback_ptr = &desc_callback;
phdc_config.info = &usb_desc_ep;
USB_mem_zero(&g_bridge, sizeof(BRIDGE_GLOBAL_VARIABLE_STRUCT));
if (_lwevent_create(&lwevent,0) != MQX_OK)
{
#if _DEBUG
printf("\nMake event failed : Main_Task");
#endif
_task_block();
}
g_usb_rx_buff_ptr = USB_mem_alloc_zero(PHDC_BULK_OUT_EP_SIZE);
if(g_usb_rx_buff_ptr == NULL)
{
#if _DEBUG
printf("g_usb_rx_buff_ptr malloc failed\n");
#endif
}
g_bridge_rx_buff_ptr = USB_mem_alloc_zero(PHDC_BULK_IN_EP_SIZE);
if(g_bridge_rx_buff_ptr == NULL)
{
#if _DEBUG
printf("g_bridge_rx_buff_ptr malloc failed\n");
#endif
}
_int_disable();
g_bridge.handle = USB_Class_PHDC_Init(&phdc_config);
Bridge_Interface_Init(Bridge_Callback);
_int_enable();
while(TRUE)
{
/* Block the task untill USB Enumeration is completed */
if(_lwevent_wait_for(&lwevent,USB_ENUM_COMPLETED,FALSE,NULL) !=
MQX_OK)
{
#if _DEBUG
printf("USB_ENUM_COMPLETEDEvent Wait failed\n");
#endif
_task_block();
}
if(_lwevent_clear(&lwevent,USB_ENUM_COMPLETED) != MQX_OK)
{
#if _DEBUG
printf("Enum Event Clear failed\n");
#endif
_task_block();
}
Bridge_Interface_Open(param);
}
}