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