static void vCheckTask( void *pvParameters ) { portBASE_TYPE xErrorOccurred = pdFALSE; TickType_t xLastExecutionTime; unsigned portBASE_TYPE uxColumn = 0; xLCDMessage xMessage; xLastExecutionTime = xTaskGetTickCount(); xMessage.xColumn = 0; xMessage.pcMessage = "PASS"; for( ;; ) { /* Perform this check every mainCHECK_DELAY milliseconds. */ vTaskDelayUntil( &xLastExecutionTime, mainCHECK_DELAY ); /* Has an error been found in any task? */ if( xAreBlockingQueuesStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xArePollingQueuesStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xIsCreateTaskStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } LCD_cls(); xMessage.xColumn++; LCD_gotoxy( ( uxColumn & 0x07 ) + 1, ( uxColumn & 0x01 ) + 1 ); if( xErrorOccurred == pdTRUE ) { xMessage.pcMessage = "FAIL"; } /* Send the message to the LCD gatekeeper for display. */ xQueueSend( xLCDQueue, &xMessage, portMAX_DELAY ); } }
static void prvCheckTask( void *pvParameters ) { static volatile unsigned long ulLastRegTest1CycleCount = 0UL, ulLastRegTest2CycleCount = 0UL; portTickType xNextWakeTime, xCycleFrequency = mainNO_ERROR_CYCLE_TIME; extern void vSetupHighFrequencyTimer( void ); /* If this is being executed then the kernel has been started. Start the high frequency timer test as described at the top of this file. This is only included in the optimised build configuration - otherwise it takes up too much CPU time. */ #ifdef INCLUDE_HIGH_FREQUENCY_TIMER_TEST vSetupHighFrequencyTimer(); #endif /* Initialise xNextWakeTime - this only needs to be done once. */ xNextWakeTime = xTaskGetTickCount(); for( ;; ) { /* Place this task in the blocked state until it is time to run again. */ vTaskDelayUntil( &xNextWakeTime, xCycleFrequency ); /* Check the standard demo tasks are running without error. */ if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { /* Increase the rate at which this task cycles, which will increase the rate at which mainCHECK_LED flashes to give visual feedback that an error has occurred. */ pcStatusMessage = "Error: GenQueue"; xPrintf( pcStatusMessage ); } if( xAreQueuePeekTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: QueuePeek\r\n"; xPrintf( pcStatusMessage ); } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { pcStatusMessage = "Error: BlockQueue\r\n"; xPrintf( pcStatusMessage ); } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: BlockTime\r\n"; xPrintf( pcStatusMessage ); } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: SemTest\r\n"; xPrintf( pcStatusMessage ); } if( xArePollingQueuesStillRunning() != pdTRUE ) { pcStatusMessage = "Error: PollQueue\r\n"; xPrintf( pcStatusMessage ); } if( xIsCreateTaskStillRunning() != pdTRUE ) { pcStatusMessage = "Error: Death\r\n"; xPrintf( pcStatusMessage ); } if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { pcStatusMessage = "Error: IntMath\r\n"; xPrintf( pcStatusMessage ); } if( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: RecMutex\r\n"; xPrintf( pcStatusMessage ); } if( xAreIntQueueTasksStillRunning() != pdPASS ) { pcStatusMessage = "Error: IntQueue\r\n"; xPrintf( pcStatusMessage ); } if( xAreMathsTaskStillRunning() != pdPASS ) { pcStatusMessage = "Error: Flop\r\n"; xPrintf( pcStatusMessage ); } /* Check the reg test tasks are still cycling. They will stop incrementing their loop counters if they encounter an error. */ if( ulRegTest1CycleCount == ulLastRegTest1CycleCount ) { pcStatusMessage = "Error: RegTest1\r\n"; xPrintf( pcStatusMessage ); } if( ulRegTest2CycleCount == ulLastRegTest2CycleCount ) { pcStatusMessage = "Error: RegTest2\r\n"; xPrintf( pcStatusMessage ); } ulLastRegTest1CycleCount = ulRegTest1CycleCount; ulLastRegTest2CycleCount = ulRegTest2CycleCount; /* Toggle the check LED to give an indication of the system status. If the LED toggles every 5 seconds then everything is ok. A faster toggle indicates an error. */ vParTestToggleLED( mainCHECK_LED ); /* Ensure the LED toggles at a faster rate if an error has occurred. */ if( pcStatusMessage != NULL ) { xCycleFrequency = mainERROR_CYCLE_TIME; } } }
char handle_soltic(char *s,int timeout) { unsigned char temp[18]; portTickType xLastWakeTime; char i=0; char status_solt=2; xLastWakeTime = xTaskGetTickCount(); #ifdef debug printf("start solt ic card!\r\n"); #endif do { i++; status_solt=PcdRequest(PICC_REQIDL,temp); vTaskDelayUntil( &xLastWakeTime, ( timeout / portTICK_RATE_MS ) ); } while((status_solt!=0)&&(i<50)); i=0; if(0==status_solt) { // #ifdef debug printf("PcdRequest OK!\r\n"); #endif do { i++; status_solt=PcdAnticoll(temp); vTaskDelayUntil( &xLastWakeTime, ( timeout / portTICK_RATE_MS ) ); } while((status_solt!=0)&&(i<10)); i=0; if(0==status_solt) { // #ifdef debug printf("PcdAnticoll is OK \r\n"); #endif memcpy(s,temp,4); buzzer(100); return 0; } else { #ifdef debug printf("PcdAnticoll error \r\n"); #endif buzzer(200); return 2; } } else { //认为是没有刷卡,或者是无效卡 #ifdef debug printf("solt ic card error!\r\n"); #endif } return 1; }
static void prvCheckTask( void *pvParameters ) { TickType_t xDelayPeriod = mainNO_ERROR_CHECK_TASK_PERIOD; TickType_t xLastExecutionTime; static unsigned long ulLastRegTest1Value = 0, ulLastRegTest2Value = 0; unsigned long ulErrorFound = pdFALSE; /* Just to stop compiler warnings. */ ( void ) pvParameters; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil() works correctly. */ xLastExecutionTime = xTaskGetTickCount(); /* Cycle for ever, delaying then checking all the other tasks are still operating without error. The onboard LED is toggled on each iteration. If an error is detected then the delay period is decreased from mainNO_ERROR_CHECK_TASK_PERIOD to mainERROR_CHECK_TASK_PERIOD. This has the effect of increasing the rate at which the onboard LED toggles, and in so doing gives visual feedback of the system status. */ for( ;; ) { /* Delay until it is time to execute again. */ vTaskDelayUntil( &xLastExecutionTime, xDelayPeriod ); /* Check all the demo tasks (other than the flash tasks) to ensure that they are all still running, and that none have detected an error. */ if( xAreIntQueueTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 0UL; } if( xAreMathsTaskStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 1UL; } if( xAreDynamicPriorityTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 2UL; } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 3UL; } if ( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 4UL; } if ( xAreGenericQueueTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 5UL; } if ( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 6UL; } if( xIsCreateTaskStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 7UL; } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 8UL; } if( xAreTimerDemoTasksStillRunning( ( TickType_t ) xDelayPeriod ) != pdPASS ) { ulErrorFound = 1UL << 9UL; } if( xAreCountingSemaphoreTasksStillRunning() != pdTRUE ) { ulErrorFound = 1UL << 10UL; } if( xIsQueueOverwriteTaskStillRunning() != pdPASS ) { ulErrorFound = 1UL << 11UL; } if( xAreEventGroupTasksStillRunning() != pdPASS ) { ulErrorFound = 1UL << 12UL; } if( xAreInterruptSemaphoreTasksStillRunning() != pdPASS ) { ulErrorFound = 1UL << 13UL; } if( xAreTaskNotificationTasksStillRunning() != pdPASS ) { ulErrorFound = 1UL << 14UL; } /* Check that the register test 1 task is still running. */ if( ulLastRegTest1Value == ulRegTest1LoopCounter ) { ulErrorFound = 1UL << 15UL; } ulLastRegTest1Value = ulRegTest1LoopCounter; /* Check that the register test 2 task is still running. */ if( ulLastRegTest2Value == ulRegTest2LoopCounter ) { ulErrorFound = 1UL << 16UL; } ulLastRegTest2Value = ulRegTest2LoopCounter; /* Toggle the check LED to give an indication of the system status. If the LED toggles every mainNO_ERROR_CHECK_TASK_PERIOD milliseconds then everything is ok. A faster toggle indicates an error. */ LED_Toggle( mainCHECK_LED ); if( ulErrorFound != pdFALSE ) { /* An error has been detected in one of the tasks - flash the LED at a higher frequency to give visible feedback that something has gone wrong (it might just be that the loop back connector required by the comtest tasks has not been fitted). */ xDelayPeriod = mainERROR_CHECK_TASK_PERIOD; } } }
void host_mass_storage_task(void) #endif { uint8_t i; uint8_t max_lun; uint32_t capacity; #ifdef FREERTOS_USED portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&xLastWakeTime, configTSK_USB_HMS_PERIOD); #endif // FREERTOS_USED // First, check the host controller is in full operating mode with the // B-device attached and enumerated if (Is_host_ready()) { #if BOARD == EVK1100 // Display Start-of-Frame counter on LEDs LED_Display_Field(LED_MONO0_GREEN | LED_MONO1_GREEN | LED_MONO2_GREEN | LED_MONO3_GREEN, sof_cnt >> 5); #elif BOARD == EVK1101 || BOARD == UC3C_EK || BOARD == EVK1104 || BOARD == EVK1105 || BOARD == UC3C_EK // Display Start-of-Frame counter on LEDs LED_Display_Field(LED0 | LED1, sof_cnt >> 5); #else #error The display of the SOFs must be defined here. #endif // New device connection (executed only once after device connection) if (ms_new_device_connected) { ms_new_device_connected = false; // For all supported interfaces for (i = 0; i < Get_nb_supported_interface(); i++) { // If mass-storage class if (Get_class(i) == MS_CLASS) { ms_connected = true; LOG_STR(log_ms_dev_connected); // Get correct physical pipes associated with IN/OUT endpoints if (Is_ep_in(i, 0)) { // Yes, associate it with the IN pipe g_pipe_ms_in = Get_ep_pipe(i, 0); g_pipe_ms_out = Get_ep_pipe(i, 1); } else { // No, invert... g_pipe_ms_in = Get_ep_pipe(i, 1); g_pipe_ms_out = Get_ep_pipe(i, 0); } // Get the number of LUNs in the connected mass-storage device max_lun = host_get_lun(); // Initialize all USB drives for (host_selected_lun = 0; host_selected_lun < max_lun; host_selected_lun++) { if( host_ms_inquiry() != CTRL_GOOD ) continue; if( host_read_capacity(host_selected_lun, &capacity) != CTRL_GOOD ) continue; if( host_ms_request_sense() == CTRL_FAIL ) continue; for (i = 0; i < 3; i++) { if (host_test_unit_ready(host_selected_lun) == CTRL_GOOD) { host_read_capacity(host_selected_lun, &capacity); break; } } } break; } } } } #ifdef FREERTOS_USED } #endif }
/*------------------------------------------------------------------------------ * Function: inputTask * * Description: This task polls PINB for the current button state to determine * if the player should turn, accelerate, or both. This task never blocks, so * it should run at the lowest priority above the idle task priority. * * param vParam: This parameter is not used. *----------------------------------------------------------------------------*/ void inputTask(void *vParam) { /* Note: * ship.accel stores if the ship is moving * ship.a_vel stores which direction the ship is moving in */ //vTaskDelay(1000/portTICK_RATE_MS); // variable to hold ticks value of last task run portTickType xLastWakeTime; // Initialize the xLastWakeTime variable with the current time. xLastWakeTime = xTaskGetTickCount(); snesInit(SNES_2P_MODE); //snesInit(player_num); while (1) { //xQueueReceive( xSnesDataQueue, &controller_data, portMAX_DELAY ); //xSemaphoreTake(xSnesMutex, portMAX_DELAY); //controller_data = snesData(player_num); DDRF = 0xFF; uint16_t controller_data1, controller_data2; controller_data1 = snesData(SNES_P1); controller_data2 = snesData(SNES_P2); //PORTF = ((controller_data1) & 0xF0)|((controller_data2>>4) & 0x0F); if(controller_data1 & SNES_LEFT_BTN) ship1.a_vel = +SHIP_AVEL; else if(controller_data1 & SNES_RIGHT_BTN) ship1.a_vel = -SHIP_AVEL; else ship1.a_vel = 0; if(controller_data1 & SNES_B_BTN) ship1.accel = SHIP_ACCEL; else { ship1.accel =0; ship1.vel.x = ship1.vel.y = 0; } if(controller_data1 & SNES_Y_BTN) fire_button1 = 1; if(controller_data2 & SNES_LEFT_BTN) ship2.a_vel = +SHIP_AVEL; else if(controller_data2 & SNES_RIGHT_BTN) ship2.a_vel = -SHIP_AVEL; else ship2.a_vel = 0; if(controller_data2 & SNES_B_BTN) ship2.accel = SHIP_ACCEL; else { ship2.vel.x = ship2.vel.y = 0; ship2.accel = 0; } if(controller_data2 & SNES_Y_BTN) fire_button2 = 1; //xSemaphoreGive(xSnesMutex); vTaskDelayUntil( &xLastWakeTime, 100/portTICK_RATE_MS); } }
static void prvCheckTask( void *pvParameters ) { unsigned portLONG ulLastRegTest1Value = 0, ulLastRegTest2Value = 0, ulTicksToWait = mainNO_ERROR_PERIOD; portTickType xLastExecutionTime; /* Buffer into which the high frequency timer count is written as a string. */ static portCHAR cStringBuffer[ mainMAX_STRING_LENGTH ]; /* The count of the high frequency timer interrupts. */ extern unsigned portLONG ulHighFrequencyTimerInterrupts; xLCDMessage xMessage = { ( 200 / portTICK_RATE_MS ), cStringBuffer }; /* Setup the high frequency, high priority, timer test. It is setup here to ensure it does not fire before the scheduler is started. */ vSetupTimerTest( mainTEST_INTERRUPT_FREQUENCY ); /* Initialise the variable used to control our iteration rate prior to its first use. */ xLastExecutionTime = xTaskGetTickCount(); for( ;; ) { /* Wait until it is time to run the tests again. */ vTaskDelayUntil( &xLastExecutionTime, ulTicksToWait ); /* Has either register check 1 or 2 task discovered an error? */ if( ulStatus1 != pdPASS ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Reg test1"; } /* Check that the register test 1 task is still running. */ if( ulLastRegTest1Value == ulRegTest1Cycles ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Reg test2"; } ulLastRegTest1Value = ulRegTest1Cycles; /* Check that the register test 2 task is still running. */ if( ulLastRegTest2Value == ulRegTest2Cycles ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Reg test3"; } ulLastRegTest2Value = ulRegTest2Cycles; /* Have any of the standard demo tasks detected an error in their operation? */ if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Gen Q"; } else if( xAreQueuePeekTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Q Peek"; } else if( xAreComTestTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: COM test"; } else if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Blck time"; } else if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Sem test"; } else if( xAreIntQueueTasksStillRunning() != pdTRUE ) { ulTicksToWait = mainERROR_PERIOD; xMessage.pcMessage = "Error: Int queue"; } /* Write the ulHighFrequencyTimerInterrupts value to the string buffer. It will only be displayed if no errors have been detected. */ sprintf( cStringBuffer, "Pass %u", ( unsigned int ) ulHighFrequencyTimerInterrupts ); xQueueSend( xLCDQueue, &xMessage, mainDONT_WAIT ); vParTestToggleLED( mainCHECK_LED ); } }
/** * * @brief Suspends execution of current thread for a regular interval. * * @param[in] previous_ms pointer to system time of last execution, * must have been initialized with PIOS_Thread_Systime() on first invocation * @param[in] increment_ms time of regular interval in milliseconds * */ void PIOS_Thread_Sleep_Until(uint32_t *previous_ms, uint32_t increment_ms) { portTickType temp = MS2TICKS(*previous_ms); vTaskDelayUntil(&temp, (portTickType)MS2TICKS(increment_ms)); *previous_ms = TICKS2MS(temp); }
static void prvRefreshTask( void *pvParameters ) { /* This is table 3.3 from the Spartan-3 Starter Kit board user guide */ const unsigned char bits[ 16 ] = { 0x01, 0x4f, 0x12, 0x06, 0x4c, 0x24, 0x20, 0x0f, 0x00, 0x04, 0x08, 0x60, 0x31, 0x42, 0x30, 0x38 }; const unsigned char apsx[4] = { 0x06, /* 3 */ 0x24, /* S */ 0x18, /* P */ 0x08 /* A */ }; portTickType xRefreshRate, xLastRefreshTime; /* Digit to scan */ static int d = 0; xRefreshRate = 2; /* We need to initialise xLastRefreshTime prior to the first call to vTaskDelayUntil(). */ xLastRefreshTime = xTaskGetTickCount(); for (;;) { for( d = 0; d < 4; d++ ) { vTaskDelayUntil ( &xLastRefreshTime, xRefreshRate ); /* Display digit */ gpio->out.an = -1; if( ( seg7_digits[ 1 ] == 4 ) || ( seg7_digits[ 1 ] == 5 ) ) { gpio->out.digit = apsx[ d ]; } else { gpio->out.digit = bits[ seg7_digits[ d ] ]; } gpio->out.dp = 1; gpio->out.an = ~(1 << d); } } }
void XMLRPCServer::UDPreceive(void* params) { struct netconn *conn; struct netbuf *buf; err_t err; // Initialize memory (in stack) for message. char message[60]; // TODO: Set its size according to UDPMessage. os_printf("Test!\n"); conn = netconn_new(NETCONN_UDP); for (;;) { // Check if connection was created successfully. if (conn != NULL) { err = netconn_bind(conn, IP_ADDR_ANY, UDP_RECEIVE_PORT); // Check if we were able to bind to port. if (err == ERR_OK) { portTickType xLastWakeTime; // Initialize the xLastWakeTime variable with the current time. xLastWakeTime = xTaskGetTickCount(); // Start periodic loop. while (1) { buf = netconn_recv(conn); if (buf != NULL) { struct ip_addr* ip; uint16_t port; ip = buf->addr; port = buf->port; //if(ip != NULL) //os_printf("Received from %d:%d!\n", ip->addr, port); // Copy received data into message. uint16_t len = netbuf_len(buf); if (len > 15) { netbuf_copy (buf, &message, len); uint32_t connectionID = *((uint32_t*)&message[0]); TopicReader* tr = getTopicReader(connectionID); if (tr != NULL) { tr->enqueueMessage(&message[8]); //os_printf("ConnectionID: %d, topic:%s\n", connectionID, tr->getTopic()); } } // Deallocate previously created memory. netbuf_delete(buf); } // Use delay until to guarantee periodic execution of each loop iteration. else { os_printf("buf = NULL!\n"); vTaskDelayUntil(&xLastWakeTime, 30); } } } else { os_printf("cannot bind netconn\n"); } } else { os_printf("cannot create new UDP netconn\n"); conn = netconn_new(NETCONN_UDP); } // If connection failed, wait for 50 ms before retrying. vTaskDelay(50); } }
void host_mouse_hid_task(void) #endif { uint8_t i; #ifdef FREERTOS_USED portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&xLastWakeTime, configTSK_USB_HHID_MOUSE_PERIOD); #endif // FREERTOS_USED // First, check the host controller is in full operating mode with the // B-device attached and enumerated if (Is_host_ready()) { // New device connection (executed only once after device connection) if (mouse_hid_new_device_connected) { mouse_hid_new_device_connected = false; // For all supported interfaces for (i = 0; i < Get_nb_supported_interface(); i++) { if(Get_class(i)==HID_CLASS && Get_protocol(i)==MOUSE_PROTOCOL) { host_hid_set_idle(HID_IDLE_DURATION_INDEFINITE, HID_REPORT_ID_ALL, i); host_hid_get_report(HID_REPORT_DESCRIPTOR, 0, i); pipe_mouse_in = Get_ep_pipe(i, 0); Host_enable_continuous_in_mode(pipe_mouse_in); Host_unfreeze_pipe(pipe_mouse_in); mouse_hid_connected=true; break; } } } if( Is_host_mouse_hid_configured() ) { if((Is_host_in_received(pipe_mouse_in)) && (Is_host_stall(pipe_mouse_in)==false) ) { Host_reset_pipe_fifo_access(pipe_mouse_in); usb_report[0]= usb_report[1]= usb_report[2]= usb_report[3]=0; host_read_p_rxpacket(pipe_mouse_in, (void*)usb_report, 4, NULL); Host_ack_in_received(pipe_mouse_in); Host_free_in(pipe_mouse_in); new_x = usb_report[1]; new_y = usb_report[2]; mouse_x += new_x; mouse_y += new_y; if( mouse_x<MOUSE_X_MIN ) mouse_x=MOUSE_X_MIN; else if( mouse_x>MOUSE_X_MAX ) mouse_x=MOUSE_X_MAX; if( mouse_y<MOUSE_Y_MIN ) mouse_y=MOUSE_Y_MIN; else if( mouse_y>MOUSE_Y_MAX ) mouse_y=MOUSE_Y_MAX; mouse_b0=usb_report[0] & 1; mouse_b1=usb_report[0] & 2; mouse_b2=usb_report[0] & 4; disp_led_mouse(); disp_ascii_mouse(); } if(Is_host_nak_received(pipe_mouse_in)) { Host_ack_nak_received(pipe_mouse_in); LED_Off(LED_HOST_MOUSE_B0 ); LED_Off(LED_HOST_MOUSE_B1 ); LED_Off(LED_HOST_MOUSE_B2 ); LED_Off(LED_HOST_MOUSE_B3 ); } } } #ifdef FREERTOS_USED } #endif }
static void prvCheckTask( void *pvParameters ) { TickType_t xNextWakeTime; const TickType_t xCycleFrequency = 2500 / portTICK_PERIOD_MS; /* Just to remove compiler warning. */ ( void ) pvParameters; /* Initialise xNextWakeTime - this only needs to be done once. */ xNextWakeTime = xTaskGetTickCount(); for( ;; ) { /* Place this task in the blocked state until it is time to run again. */ vTaskDelayUntil( &xNextWakeTime, xCycleFrequency ); /* Check the standard demo tasks are running without error. */ #if( configUSE_PREEMPTION != 0 ) { /* These tasks are only created when preemption is used. */ if( xAreTimerDemoTasksStillRunning( xCycleFrequency ) != pdTRUE ) { pcStatusMessage = "Error: TimerDemo"; } } #endif if( xAreEventGroupTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: EventGroup"; } else if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { pcStatusMessage = "Error: IntMath"; } else if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: GenQueue"; } else if( xAreQueuePeekTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: QueuePeek"; } else if( xAreBlockingQueuesStillRunning() != pdTRUE ) { pcStatusMessage = "Error: BlockQueue"; } else if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: SemTest"; } else if( xArePollingQueuesStillRunning() != pdTRUE ) { pcStatusMessage = "Error: PollQueue"; } else if( xAreMathsTaskStillRunning() != pdPASS ) { pcStatusMessage = "Error: Flop"; } else if( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: RecMutex"; } else if( xAreCountingSemaphoreTasksStillRunning() != pdTRUE ) { pcStatusMessage = "Error: CountSem"; } else if( xIsCreateTaskStillRunning() != pdTRUE ) { pcStatusMessage = "Error: Death"; } else if( xAreDynamicPriorityTasksStillRunning() != pdPASS ) { pcStatusMessage = "Error: Dynamic"; } else if( xAreQueueSetTasksStillRunning() != pdPASS ) { pcStatusMessage = "Error: Queue set"; } else if( xIsQueueOverwriteTaskStillRunning() != pdPASS ) { pcStatusMessage = "Error: Queue overwrite"; } /* This is the only task that uses stdout so its ok to call printf() directly. */ printf( "%s - %d\r\n", pcStatusMessage, xTaskGetTickCount() ); } }
void usb_host_task(void) #endif { #define DEVICE_DEFAULT_MAX_ERROR_COUNT 2 static uint8_t device_default_error_count; #ifdef HOST_VBUS_LOW_TIMEOUT extern t_cpu_time timer_vbus_low; #endif static bool sav_int_sof_enable; uint8_t pipe; #ifdef FREERTOS_USED portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&xLastWakeTime, configTSK_USB_HST_PERIOD); #endif // FREERTOS_USED switch (device_state) { #ifdef HOST_VBUS_LOW_TIMEOUT case DEVICE_VBUS_LOW: Usb_disable_vbus(); if (cpu_is_timeout(&timer_vbus_low)) usb_host_task_init(); break; #endif //------------------------------------------------------ // DEVICE_UNATTACHED state // // - Default init state // - Try to give device power supply // case DEVICE_UNATTACHED: device_default_error_count = 0; nb_interface_supported = 0; Host_clear_device_status(); // Reset device status Usb_clear_all_event(); // Clear all software events Host_disable_sof(); host_disable_all_pipes(); Usb_enable_vbus(); // Give at least device power supply! // If VBus OK, wait for device connection if (Is_usb_vbus_high()) device_state = DEVICE_ATTACHED; break; //------------------------------------------------------ // DEVICE_ATTACHED state // // - VBus is on // - Try to detect device connection // case DEVICE_ATTACHED: if (Is_host_device_connection() || Is_usb_event(EVT_HOST_CONNECTION) ) // Device pull-up detected { device_attached_retry: if( Is_usb_event(EVT_HOST_CONNECTION) ) { Usb_ack_event(EVT_HOST_CONNECTION); } Usb_ack_bconnection_error_interrupt(); Usb_ack_vbus_error_interrupt(); Host_ack_device_connection(); Host_clear_device_status(); // Reset device status cpu_irq_disable(); Host_disable_device_disconnection_interrupt(); Host_send_reset(); // First USB reset (void)Is_host_sending_reset(); cpu_irq_enable(); Usb_ack_event(EVT_HOST_SOF); // Active wait for end of reset send while (Is_host_sending_reset()) { // The USB macro does not signal the end of reset when a disconnection occurs if (Is_host_device_disconnection()) { // Stop sending USB reset Host_stop_sending_reset(); } } Host_ack_reset_sent(); Host_enable_sof(); // Start SOF generation Host_enable_sof_interrupt(); // SOF will be detected under interrupt if (!Is_host_device_disconnection()) { // Workaround for some buggy devices with powerless pull-up // usually low-speed where data line rises slowly and can be interpreted as disconnection for (sof_cnt = 0; sof_cnt < 0xFFFF; sof_cnt++) // Basic time-out counter { // If we detect SOF, device is still alive and connected, just clear false disconnect flag if (Is_usb_event(EVT_HOST_SOF) && Is_host_device_disconnection()) { Host_ack_device_connection(); Host_ack_device_disconnection(); break; } } } Host_enable_device_disconnection_interrupt(); sof_cnt = 0; while (sof_cnt < 100) // Wait 100 ms after USB reset { if (Is_usb_event(EVT_HOST_SOF)) Usb_ack_event(EVT_HOST_SOF), sof_cnt++; // Count SOFs if (Is_host_emergency_exit() || Is_usb_bconnection_error_interrupt()) goto device_attached_error; } device_state = DEVICE_POWERED; LOG_STR(log_device_connected); Host_device_connection_action(); sof_cnt = 0; } device_attached_error: // Device connection error, or VBus pb -> Retry the connection process from the beginning if (Is_usb_bconnection_error_interrupt() || Is_usb_vbus_error_interrupt() || Is_usb_vbus_low()) { if (device_state != DEVICE_VBUS_LOW) device_state = DEVICE_UNATTACHED; Usb_ack_bconnection_error_interrupt(); Usb_ack_vbus_error_interrupt(); Host_disable_sof(); } break; //------------------------------------------------------ // DEVICE_POWERED state // // - Device connection (attach) has been detected, // - Wait 100 ms and configure default control pipe // case DEVICE_POWERED: if (Is_usb_event(EVT_HOST_SOF)) { Usb_ack_event(EVT_HOST_SOF); if (sof_cnt++ >= 100) // Wait 100 ms { Host_enable_pipe(P_CONTROL); (void)Host_configure_pipe(P_CONTROL, 0, EP_CONTROL, TYPE_CONTROL, TOKEN_SETUP, 8, SINGLE_BANK); device_state = DEVICE_DEFAULT; } } break; //------------------------------------------------------ // DEVICE_DEFAULT state // // - Get device descriptor // - Reconfigure control pipe according to device control endpoint // - Assign device address // case DEVICE_DEFAULT: // Get first device descriptor if (host_get_device_descriptor_incomplete() == CONTROL_GOOD) { sof_cnt = 0; while (sof_cnt < 20) // Wait 20 ms before USB reset (special buggy devices...) { if (Is_usb_event(EVT_HOST_SOF)) Usb_ack_event(EVT_HOST_SOF), sof_cnt++; if (Is_host_emergency_exit() || Is_usb_bconnection_error_interrupt()) break; } cpu_irq_disable(); Host_disable_device_disconnection_interrupt(); Host_send_reset(); // First USB reset (void)Is_host_sending_reset(); cpu_irq_enable(); Usb_ack_event(EVT_HOST_SOF); // Active wait for end of reset send while (Is_host_sending_reset()) { // The USB macro does not signal the end of reset when a disconnection occurs if (Is_host_device_disconnection()) { // Stop sending USB reset Host_stop_sending_reset(); } } Host_ack_reset_sent(); if (!Is_host_device_disconnection()) { // Workaround for some buggy devices with powerless pull-up // usually low-speed where data line rises slowly and can be interpreted as disconnection for (sof_cnt = 0; sof_cnt < 0xFFFF; sof_cnt++) // Basic time-out counter { // If we detect SOF, device is still alive and connected, just clear false disconnect flag if (Is_usb_event(EVT_HOST_SOF) && Is_host_device_disconnection()) { Host_ack_device_connection(); Host_ack_device_disconnection(); break; } } } Host_enable_device_disconnection_interrupt(); sof_cnt = 0; while (sof_cnt < 200) // Wait 200 ms after USB reset { if (Is_usb_event(EVT_HOST_SOF)) Usb_ack_event(EVT_HOST_SOF), sof_cnt++; if (Is_host_emergency_exit() || Is_usb_bconnection_error_interrupt()) break; } Host_disable_pipe(P_CONTROL); Host_unallocate_memory(P_CONTROL); Host_enable_pipe(P_CONTROL); // Reconfigure the control pipe according to the device control endpoint (void)Host_configure_pipe(P_CONTROL, 0, EP_CONTROL, TYPE_CONTROL, TOKEN_SETUP, data_stage[OFFSET_FIELD_MAXPACKETSIZE], SINGLE_BANK); // Give an absolute device address if (host_set_address(DEVICE_ADDRESS) == CONTROL_GOOD) { for (pipe = 0; pipe < MAX_PEP_NB; pipe++) Host_configure_address(pipe, DEVICE_ADDRESS); device_state = DEVICE_ADDRESSED; } else if (device_state != DEVICE_VBUS_LOW) device_state = DEVICE_ERROR; } else { if (device_state != DEVICE_VBUS_LOW) { if (++device_default_error_count > DEVICE_DEFAULT_MAX_ERROR_COUNT) device_state = DEVICE_ERROR; else { Host_disable_sof(); Host_disable_pipe(P_CONTROL); Host_unallocate_memory(P_CONTROL); device_state = DEVICE_ATTACHED; goto device_attached_retry; } } Usb_ack_bconnection_error_interrupt(); Usb_ack_vbus_error_interrupt(); Host_disable_sof(); } break; //------------------------------------------------------ // DEVICE_ADDRESSED state // // - Check if VID PID is in supported list // case DEVICE_ADDRESSED: if (host_get_device_descriptor() == CONTROL_GOOD) { // Detect if the device connected belongs to the supported devices table if (host_check_VID_PID()) { Host_set_device_supported(); Host_device_supported_action(); device_state = DEVICE_CONFIGURED; } else { #if HOST_STRICT_VID_PID_TABLE == ENABLE device_state = DEVICE_ERROR; LOG_STR(log_unsupported_device); #else device_state = DEVICE_CONFIGURED; #endif Host_device_not_supported_action(); } } else if (device_state != DEVICE_VBUS_LOW) device_state = DEVICE_ERROR; // Can not get device descriptor break; //------------------------------------------------------ // DEVICE_CONFIGURED state // // - Configure pipes for the supported interface // - Send Set_configuration() request // - Go to full operating mode (device ready) // case DEVICE_CONFIGURED: { uint8_t configuration_index = 0; if (host_get_configuration_descriptor(configuration_index) == CONTROL_GOOD) { if (host_check_class()) // Class support OK? { #if HOST_AUTO_CFG_ENDPOINT == DISABLE User_configure_endpoint(); // User call here instead of autoconfig Host_set_configured(); // Assumes config is OK with user config #endif if (Is_host_configured()) { if (host_set_configuration(data_stage[OFFSET_FIELD_CONFIGURATION_NB]) == CONTROL_GOOD) // Send Set_configuration { // Device and host are now fully configured // go to DEVICE_READY normal operation device_state = DEVICE_READY; // Monitor device disconnection under interrupt Host_enable_device_disconnection_interrupt(); // If user host application requires SOF interrupt event // Keep SOF interrupt enabled, otherwise disable this interrupt #if HOST_CONTINUOUS_SOF_INTERRUPT == DISABLE cpu_irq_disable(); Host_disable_sof_interrupt(); (void)Is_host_sof_interrupt_enabled(); cpu_irq_enable(); #endif Host_new_device_connection_action(); cpu_irq_enable(); LOG_STR(log_device_enumerated); } else if (device_state != DEVICE_VBUS_LOW) device_state = DEVICE_ERROR; // Problem during Set_configuration request... } } else // Device class not supported... { device_state = DEVICE_UNSUPPORTED; LOG_STR(log_unsupported_device); Host_device_class_not_supported_action(); } } else if (device_state != DEVICE_VBUS_LOW) device_state = DEVICE_ERROR; // Can not get configuration descriptors... } break; //------------------------------------------------------ // DEVICE_READY state // // - Full standard operating mode // - Nothing to do... // case DEVICE_READY: // Host full standard operating mode! break; //------------------------------------------------------ // DEVICE_UNSUPPORTED state // case DEVICE_UNSUPPORTED: break; //------------------------------------------------------ // DEVICE_ERROR state // // - Error state // - Do custom action call (probably go to default mode...) // case DEVICE_ERROR: //! @todo #if HOST_ERROR_RESTART == ENABLE device_state = DEVICE_UNATTACHED; #endif Host_device_error_action(); break; //------------------------------------------------------ // DEVICE_SUSPENDED state // // - Host application request to suspend the device activity // - State machine comes here thanks to Host_request_suspend() // case DEVICE_SUSPENDED: if (Is_device_supports_remote_wakeup()) // If the connected device supports remote wake-up { host_set_feature_remote_wakeup(); // Enable this feature... } LOG_STR(log_usb_suspended); sav_int_sof_enable = Is_host_sof_interrupt_enabled(); //Save current SOF interrupt enable state cpu_irq_disable(); Host_disable_sof_interrupt(); (void)Is_host_sof_interrupt_enabled(); cpu_irq_enable(); Host_ack_sof(); Host_disable_sof(); // Stop SOF generation, this generates the suspend state Host_ack_hwup(); Host_enable_hwup_interrupt(); // Enable host wake-up interrupt // (this is the unique USB interrupt able to wake up the CPU core from power-down mode) (void)Is_host_hwup_interrupt_enabled(); // Make sure host wake-up interrupt is enabled Usb_freeze_clock(); //! @todo Implement this on the silicon version //Stop_pll(); Host_suspend_action(); // Custom action here! (e.g. go to power-save mode...) device_state = DEVICE_WAIT_RESUME; // Wait for device resume event break; //------------------------------------------------------ // DEVICE_WAIT_RESUME state // // Wait in this state till: // - the host receives an upstream resume from the device // - or the host software request the device to resume // case DEVICE_WAIT_RESUME: if (Is_usb_event(EVT_HOST_HWUP) || Is_host_request_resume()) // Remote wake-up has been detected // or local resume request has been received { if (Is_host_request_resume()) // Not a remote wake-up, but a host application request { // CAUTION: HWUP can be cleared only when USB clock is active //! @todo Implement this on the silicon version //Pll_start_auto(); // First Restart the PLL for USB operation //Wait_pll_ready(); // Make sure PLL is locked Usb_unfreeze_clock(); // Enable clock on USB interface (void)Is_usb_clock_frozen(); // Make sure USB interface clock is enabled cpu_irq_disable(); Host_disable_hwup_interrupt(); // Wake-up interrupt should be disabled as host is now awoken! (void)Is_host_hwup_interrupt_enabled(); cpu_irq_enable(); Host_ack_hwup(); // Clear HWUP interrupt flag } Host_enable_sof(); Host_send_resume(); // Send downstream resume while (!Is_host_down_stream_resume()); // Wait for downstream resume sent Host_ack_remote_wakeup(); // Ack remote wake-up reception Host_ack_request_resume(); // Ack software request Host_ack_down_stream_resume(); // Ack downstream resume sent Usb_ack_event(EVT_HOST_HWUP); // Ack software event if (sav_int_sof_enable) Host_enable_sof_interrupt(); // Restore SOF interrupt enable state before suspend device_state = DEVICE_READY; // Come back to full operating mode LOG_STR(log_usb_resumed); } break; //------------------------------------------------------ // default state // // - Default case: ERROR // - Go to DEVICE_UNATTACHED state // default: device_state = DEVICE_UNATTACHED; break; } #ifdef FREERTOS_USED } #endif }
// This is the actual task that is run static portTASK_FUNCTION( SomeTask, pvParameters ) { portTickType xUpdateRate, xLastUpdateTime; const uint8_t i2cCmdReadVals[]= {0xAA}; uint8_t temp1, rxLen, status; uint8_t messageReceived[2]; // Get the parameters i2cTempStruct *param = (i2cTempStruct *) pvParameters; // Get the I2C device pointer vtI2CStruct *devPtr = param->dev; // Get the LCD information pointer vtLCDStruct *lcdData = param->lcdData; vtLCDMsg lcdBuffer; vTaskDelay(10/portTICK_RATE_MS); xUpdateRate = i2cREAD_RATE_BASE / portTICK_RATE_MS; /* We need to initialise xLastUpdateTime prior to the first call to vTaskDelayUntil(). */ xLastUpdateTime = xTaskGetTickCount(); for(;;) { //delay for some amount of time before looping again vTaskDelayUntil( &xLastUpdateTime, xUpdateRate ); int i = 0; //toggle the bottom LED uint8_t ulCurrentState = GPIO2->FIOPIN; if( ulCurrentState & 0x40 ) { GPIO2->FIOCLR = 0x40; } else { GPIO2->FIOSET = 0x40; } //Ask for message from I2C if (vtI2CEnQ(devPtr,0x01,0x48,sizeof(i2cCmdReadVals),0x00/*i2cCmdReadVals*/,2) != pdTRUE) { VT_HANDLE_FATAL_ERROR(0); } //wait for message from I2C if (vtI2CDeQ(devPtr,2,&messageReceived[0],&rxLen,&status) != pdTRUE) { VT_HANDLE_FATAL_ERROR(0); } uint8_t testint = 0; //load the read message from I2C into the lcd Buffer lcdBuffer.buf[0] = messageReceived[1]; lcdBuffer.buf[1] = messageReceived[0]; if (lcdData != NULL && lcdBuffer.buf[0] != 0xFF) { // Send a message to the LCD task for it to print (and the LCD task must be configured to receive this message) lcdBuffer.length = strlen((char*)(lcdBuffer.buf))+1; if (xQueueSend(lcdData->inQ,(void *) (&lcdBuffer),portMAX_DELAY) != pdTRUE) { VT_HANDLE_FATAL_ERROR(0); } } } }
static void vCheckTask( void *pvParameters ) { /* Used to wake the task at the correct frequency. */ TickType_t xLastExecutionTime; /* The maximum jitter time measured by the fast interrupt test. */ extern unsigned short usMaxJitter ; /* Buffer into which the maximum jitter time is written as a string. */ static char cStringBuffer[ mainMAX_STRING_LENGTH ]; /* The message that is sent on the queue to the LCD task. The first parameter is the minimum time (in ticks) that the message should be left on the LCD without being overwritten. The second parameter is a pointer to the message to display itself. */ xLCDMessage xMessage = { 0, cStringBuffer }; /* Set to pdTRUE should an error be detected in any of the standard demo tasks. */ unsigned short usErrorDetected = pdFALSE; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil() works correctly. */ xLastExecutionTime = xTaskGetTickCount(); for( ;; ) { /* Wait until it is time for the next cycle. */ vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_PERIOD ); /* Has an error been found in any of the standard demo tasks? */ if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE; sprintf( cStringBuffer, "FAIL #1" ); } if( xAreComTestTasksStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE; sprintf( cStringBuffer, "FAIL #2" ); } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE; sprintf( cStringBuffer, "FAIL #3" ); } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE; sprintf( cStringBuffer, "FAIL #4" ); } if( usErrorDetected == pdFALSE ) { /* No errors have been discovered, so display the maximum jitter timer discovered by the "fast interrupt test". */ sprintf( cStringBuffer, "%dns max jitter", ( short ) ( usMaxJitter - mainEXPECTED_CLOCKS_BETWEEN_INTERRUPTS ) * mainNS_PER_CLOCK ); } /* Send the message to the LCD gatekeeper for display. */ xQueueSend( xLCDQueue, &xMessage, portMAX_DELAY ); } }
/** * Module thread, should not return. */ static void airspeedTask(void *parameters) { AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); BaroAirspeedData airspeedData; AirspeedActualData airspeedActualData; airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; #ifdef BARO_AIRSPEED_PRESENT portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop float airspeedErrInt=0; #endif //GPS airspeed calculation variables #ifdef GPS_AIRSPEED_PRESENT GPSVelocityConnectCallback(GPSVelocityUpdatedCb); gps_airspeedInitialize(); #endif // Main task loop portTickType lastSysTime = xTaskGetTickCount(); while (1) { // Update the airspeed object BaroAirspeedGet(&airspeedData); #ifdef BARO_AIRSPEED_PRESENT float airspeed_tas_baro=0; if(airspeedSensorType != AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) { //Fetch calibrated airspeed from sensors baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin); //Calculate true airspeed, not taking into account compressibility effects int16_t groundTemperature_10; float groundTemperature; float positionActual_Down; PositionActualDownGet(&positionActual_Down); HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C groundTemperature = groundTemperature_10/10; // Convert into degrees C groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm) struct AirParameters air_STP = initialize_air_structure(); air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN; #ifdef GPS_AIRSPEED_PRESENT //GPS present, so use baro sensor to filter TAS airspeed_tas_baro = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI; #else //No GPS, so TAS comes only from baro sensor airspeedData.TrueAirspeed = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI; #endif } else #endif { //Have to catch the fallthrough, or else this loop will monopolize the processor! airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE; airspeedData.SensorValue=12345; //Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH)); } #ifdef GPS_AIRSPEED_PRESENT float v_air_GPS=-1.0f; //Check if sufficient time has passed. This will depend on whether we have a pitot tube //sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS //as quickly as possible. #ifdef BARO_AIRSPEED_PRESENT float delT = TICKS2MS(lastSysTime - lastLoopTime) / 1000.0f; lastLoopTime=lastSysTime; if ( (TICKS2MS(lastSysTime - lastGPSTime) > 1000 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) && gpsNew) { lastGPSTime=lastSysTime; #else if (gpsNew) { #endif gpsNew=false; //Do this first //Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani gps_airspeedGet(&v_air_GPS); } //Use the GPS error to correct the airspeed estimate. if (v_air_GPS > 0) //We have valid GPS estimate... { airspeedData.GPSAirspeed=v_air_GPS; #ifdef BARO_AIRSPEED_PRESENT if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present... //Calculate error and error integral float airspeedErr=v_air_GPS - airspeed_tas_baro; airspeedErrInt+=airspeedErr * delT; //Saturate integral component at 5 m/s airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt; airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt; //There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function. airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP; /* Note: This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However, this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't know how to control for temperature bias. */ } else #endif { //...there's no airspeed sensor, so everything comes from GPS. In this //case, filter the airspeed for smoother output float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha); //Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor int16_t groundTemperature_10; float groundTemperature; float positionActual_Down; PositionActualDownGet(&positionActual_Down); HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C groundTemperature = groundTemperature_10/10; // Convert into degrees C groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm) struct AirParameters air_STP = initialize_air_structure(); air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN; airspeedData.CalibratedAirspeed = tas2cas(airspeedData.TrueAirspeed, -positionActual_Down, &air_STP); } } #ifdef BARO_AIRSPEED_PRESENT else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter. airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha); } #endif #endif //Set the UAVO airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed; airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed; BaroAirspeedSet(&airspeedData); AirspeedActualSet(&airspeedActualData); } } #ifdef GPS_AIRSPEED_PRESENT static void GPSVelocityUpdatedCb(UAVObjEvent * ev) { gpsNew=true; } #endif #ifdef BARO_AIRSPEED_PRESENT void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin_dummy) { //Find out which sensor we're using. switch (airspeedSensorType) { case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: //MPXV5004 and MPXV7002 sensors baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); break; case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: //Eagletree Airspeed v3 baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); break; default: baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH)); break; } } #endif static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev) { AirspeedSettingsData airspeedSettingsData; AirspeedSettingsGet(&airspeedSettingsData); airspeedSensorType=airspeedSettingsData.AirspeedSensorType; gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms; #if defined(PIOS_INCLUDE_MPXV7002) if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){ PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot. } #endif #if defined(PIOS_INCLUDE_MPXV5004) if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot. } #endif }
static void vCheckTask( void *pvParameters ) { portBASE_TYPE xErrorOccurred = pdFALSE; portTickType xLastExecutionTime; const char * const pcPassMessage = "PASS\n"; const char * const pcFailMessage = "FAIL\n"; /* Just to remove compiler warnings. */ ( void ) pvParameters; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil() works correctly. */ xLastExecutionTime = xTaskGetTickCount(); for( ;; ) { /* Perform this check every mainCHECK_DELAY milliseconds. */ vTaskDelayUntil( &xLastExecutionTime, mainCHECK_DELAY ); /* Has an error been found in any task? */ if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xArePollingQueuesStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreDynamicPriorityTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } #if configUSE_PREEMPTION == 1 { /* The timing of console output when not using the preemptive scheduler causes the block time tests to detect a timing problem. */ if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } } #endif if( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { xErrorOccurred = pdTRUE; } /* Send either a pass or fail message. If an error is found it is never cleared again. */ if( xErrorOccurred == pdTRUE ) { xLED_Delay = mainERROR_LED_DELAY; xQueueSend( xPrintQueue, &pcFailMessage, portMAX_DELAY ); } else { xQueueSend( xPrintQueue, &pcPassMessage, portMAX_DELAY ); } } }
///////////////////////////////////////////////////////////////////////////// // The uIP Task is executed each mS ///////////////////////////////////////////////////////////////////////////// static void UIP_TASK_Handler(void *pvParameters) { int i; struct timer periodic_timer, arp_timer; // take over exclusive access to UIP functions MUTEX_UIP_TAKE; // init uIP timers timer_set(&periodic_timer, CLOCK_SECOND / 2); timer_set(&arp_timer, CLOCK_SECOND * 10); // init the network driver network_device_init(); // init uIP uip_init(); uip_arp_init(); // set my ethernet address unsigned char *mac_addr = network_device_mac_addr(); { int i; for(i=0; i<6; ++i) uip_ethaddr.addr[i] = mac_addr[i]; } // enable dhcp mode (can be changed during runtime) UIP_TASK_DHCP_EnableSet(dhcp_enabled); // release exclusive access to UIP functions MUTEX_UIP_GIVE; #if 0 // wait until HW config has been loaded do { vTaskDelay(1 / portTICK_RATE_MS); } while( !SEQ_FILE_HW_ConfigLocked() ); #endif // Initialise the xLastExecutionTime variable on task entry portTickType xLastExecutionTime = xTaskGetTickCount(); // endless loop while( 1 ) { #if 0 do { vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS); } while( TASK_MSD_EnableGet() ); // don't service ethernet if MSD mode enabled for faster transfer speed #else vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS); #endif // take over exclusive access to UIP functions MUTEX_UIP_TAKE; if( !(clock_time_tick() % 100) ) { // each 100 mS: check availablility of network device #if defined(MIOS32_BOARD_MBHP_CORE_LPC17) || defined(MIOS32_BOARD_LPCXPRESSO) network_device_check(); // TK: on STM32 no auto-detection for MBSEQ for best performance if no MBHP_ETH module connected // the user has to reboot MBSEQ to restart module detection #endif } if( network_device_available() ) { uip_len = network_device_read(); if( uip_len > 0 ) { if(BUF->type == HTONS(UIP_ETHTYPE_IP) ) { uip_arp_ipin(); uip_input(); /* If the above function invocation resulted in data that should be sent out on the network, the global variable uip_len is set to a value > 0. */ if( uip_len > 0 ) { uip_arp_out(); network_device_send(); } } else if(BUF->type == HTONS(UIP_ETHTYPE_ARP)) { uip_arp_arpin(); /* If the above function invocation resulted in data that should be sent out on the network, the global variable uip_len is set to a value > 0. */ if(uip_len > 0) { network_device_send(); } } } else if(timer_expired(&periodic_timer)) { timer_reset(&periodic_timer); for(i = 0; i < UIP_CONNS; i++) { uip_periodic(i); /* If the above function invocation resulted in data that should be sent out on the network, the global variable uip_len is set to a value > 0. */ if(uip_len > 0) { uip_arp_out(); network_device_send(); } } #if UIP_UDP for(i = 0; i < UIP_UDP_CONNS; i++) { uip_udp_periodic(i); /* If the above function invocation resulted in data that should be sent out on the network, the global variable uip_len is set to a value > 0. */ if(uip_len > 0) { uip_arp_out(); network_device_send(); } } #endif /* UIP_UDP */ /* Call the ARP timer function every 10 seconds. */ if(timer_expired(&arp_timer)) { timer_reset(&arp_timer); uip_arp_timer(); } } } // release exclusive access to UIP functions MUTEX_UIP_GIVE; } }
void taskHousekeeping(void *param) { #if SCH_THOUSEKEEPING_VERBOSE printf(">>[Housekeeping] Started\r\n"); #endif portTickType delay_ms = 10000; //Task period in [ms] portTickType xDelay_ticks = delay_ms / portTICK_RATE_MS; //Task period in ticks #if (SCH_THOUSEKEEPING_REALTIME == 1) unsigned int elapsed_sec = 0; // Seconds counter unsigned int elapsed_hrs = 0; // Hours counter unsigned int check_20sec = 20; //20[s] condition unsigned int check_1min = 1*60; //1[m] condition unsigned int check_5min = 5*60; //5[m] condition unsigned int check_1hour = 60*60; //1[h] condition unsigned int check_1day = 24; //24[hrs] condition #else unsigned int elapsed_sec = 0; // Seconds counter unsigned int elapsed_hrs = 0; // Hours counter unsigned int check_20sec = 2; //2[s] condition unsigned int check_1min = 3*check_20sec; //3[s] condition unsigned int check_5min = 3*3*check_20sec; //5[s] condition unsigned int check_1hour = 3*3*3*check_20sec; //10[s] condition unsigned int check_1day = 3; //1[m] condition #endif DispCmd NewCmd; NewCmd.idOrig = SCH_THOUSEKEEPING_IDORIG; /* Housekeeping */ NewCmd.cmdId = CMD_CMDNULL; NewCmd.param = 0; //deploy if necessary //if( sta_get_BusStateVar(sta_dep_ant_deployed) == 0 ){ #if (SCH_THOUSEKEEPING_SILENT_REALTIME==1) NewCmd.cmdId = thk_id_suchai_deployment; NewCmd.param = 31; //in minutes xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); #else NewCmd.cmdId = thk_id_suchai_deployment; NewCmd.param = 3; //in minutes xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); #endif //} /*Avoid the acummulation of commands while the SUCHAI is still deploying.. */ portTickType xLastWakeTime = xTaskGetTickCount(); while(TRUE) { vTaskDelayUntil(&xLastWakeTime, xDelay_ticks); //Suspend task elapsed_sec += delay_ms/1000; //Update seconds counts /* Check if the next tick to wake has already * expired (*pxPreviousWakeTime = xTimeToWake;) * This avoids multiple reentries on vTaskDelayUntil */ BOOL xShouldDelay = shouldDelayTask(&xLastWakeTime, xDelay_ticks); if( xShouldDelay == FALSE ) { xLastWakeTime = xTaskGetTickCount(); #if (SCH_TFLIGHTPLAN2_VERBOSE>=1) printf("[Housekeeping] xLastWakeTime + xDelay_ticks < xTickCount, " "update xLastWakeTime to xTickCount ..\r\n"); #endif } //Add commands below .. /* 20 seconds actions */ if((elapsed_sec % check_20sec) == 0) { #if (SCH_THOUSEKEEPING_VERBOSE>=2) printf("[Houskeeping]: 20[s] actions ..\r\n"); #endif //Add commands below .. } /* 1 minute actions */ if((elapsed_sec % check_1min) == 0) { #if (SCH_THOUSEKEEPING_VERBOSE>=2) printf("[Houskeeping] 1[min] actions ..\r\n"); #endif //Add commands below .. } /* 5 minutes actions */ if((elapsed_sec % check_5min) == 0) { #if (SCH_THOUSEKEEPING_VERBOSE>=1) printf("[Houskeeping] 5[min] actions ..\r\n"); #endif //Add commands below .. // //print StateVars, debug purposes // NewCmd.cmdId = srp_id_print_STA_stateVar; // NewCmd.param = 1; // xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); } /* 1 hour actions */ if((elapsed_sec % check_1hour) == 0) { elapsed_hrs++; elapsed_sec = 0; //Prevent overflow #if (SCH_THOUSEKEEPING_VERBOSE>=1) printf("[Houskeeping] 1[hr] actions ..\r\n"); #endif //Add commands below .. //test trx ping NewCmd.cmdId = trx_id_isAlive; NewCmd.param = 0; xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); //test eps ping NewCmd.cmdId = eps_id_isAlive; NewCmd.param = 0; xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); // hoursWithoutReset++ NewCmd.cmdId = srp_id_increment_STA_stateVar_hoursWithoutReset; NewCmd.param = 0; xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); // hoursAlive ++ NewCmd.cmdId = srp_id_increment_STA_stateVar_hoursAlive; NewCmd.param = 0; xQueueSend(dispatcherQueue, &NewCmd, portMAX_DELAY); } /* codigo para _1day_check */ if( (elapsed_hrs % check_1day == 0) && (elapsed_hrs != 0) ) { #if (SCH_THOUSEKEEPING_VERBOSE>=1) printf("[Houskeeping]: 1[day] actions ..\r\n"); #endif elapsed_hrs = 0; //Prevent overflow //Add commands below .. } } }
void device_full_custom_task(void) #endif { U32 time=0; bool startup=true; #ifdef FREERTOS_USED portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&xLastWakeTime, configTSK_USB_DFC_PERIOD); if( startup ) { time+=configTSK_USB_DFC_PERIOD; #define STARTUP_LED_DELAY 100 if ( time== 1*STARTUP_LED_DELAY ) LED_On( LED_MONO0_GREEN ); else if( time== 2*STARTUP_LED_DELAY ) LED_On( LED_MONO1_GREEN ); else if( time== 3*STARTUP_LED_DELAY ) LED_On( LED_MONO2_GREEN ); else if( time== 4*STARTUP_LED_DELAY ) LED_On( LED_MONO3_GREEN ); else if( time== 5*STARTUP_LED_DELAY ) LED_Off( LED_MONO0_GREEN ); else if( time== 6*STARTUP_LED_DELAY ) LED_Off( LED_MONO1_GREEN ); else if( time== 7*STARTUP_LED_DELAY ) LED_Off( LED_MONO2_GREEN ); else if( time== 8*STARTUP_LED_DELAY ) LED_Off( LED_MONO3_GREEN ); else if( time== 9*STARTUP_LED_DELAY ) startup=false; } // First, check the device enumeration state if (!Is_device_enumerated()) continue; #else // First, check the device enumeration state if (!Is_device_enumerated()) return; #endif // FREERTOS_USED if(Is_usb_out_received(EP_FC_OUT)) { U32 nchar; Usb_reset_endpoint_fifo_access(EP_FC_OUT); memset(rxbuf, 0, RXBUF_SIZE); usb_read_ep_rxpacket(EP_FC_OUT, &rxbuf, RXBUF_SIZE, NULL); Usb_ack_out_received_free(EP_FC_OUT); //printf("Received %s\n\r", rxbuf); if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=temp") ) { // Temperature sensor nchar=build_answer(txbuf, "temp"); b_temperature_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=js") ) { // Joystick nchar=build_answer(txbuf, "js"); b_joystick_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=pb1") ) { // Push button 1 nchar=build_answer(txbuf, "pb1"); b_pushb1_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=pb2") ) { // Push button 2 nchar=build_answer(txbuf, "pb2"); b_pushb2_get_value( txbuf+nchar ); b_report_pending=true; } #if BOARD == EVK1100 else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=pb3") ) { // Push button 3 nchar=build_answer(txbuf, "pb3"); sprintf( txbuf+nchar, "RELEASE\r\n"); b_report_pending=true; } #endif else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=light") ) { // light U32 value; nchar=build_answer(txbuf, "light"); b_light_get_value( txbuf+nchar, &value ); e_ledm_refresh_intensity( value ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_actuator_value actuator=ledm1") ) { // led1 nchar=build_answer(txbuf, "ledm1"); b_ledm1_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_actuator_value actuator=ledm2") ) { // led2 nchar=build_answer(txbuf, "ledm2"); b_ledm2_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_actuator_value actuator=ledm3") ) { // led3 nchar=build_answer(txbuf, "ledm3"); b_ledm3_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_actuator_value actuator=ledm4") ) { // led4 nchar=build_answer(txbuf, "ledm4"); b_ledm4_get_value( txbuf+nchar ); b_report_pending=true; } else if( !strncmp((const char*)rxbuf, STR_SET_LEDM1, strlen(STR_SET_LEDM1)) ) { // led1 nchar=build_answer(txbuf, "ledm1"); e_ledm1_set_value(rxbuf+strlen(STR_SET_LEDM1), txbuf+nchar); b_report_pending=true; } else if( !strncmp((const char*)rxbuf, STR_SET_LEDM2, strlen(STR_SET_LEDM2)) ) { // led2 nchar=build_answer(txbuf, "ledm2"); e_ledm2_set_value(rxbuf+strlen(STR_SET_LEDM2), txbuf+nchar); b_report_pending=true; } else if( !strncmp((const char*)rxbuf, STR_SET_LEDM3, strlen(STR_SET_LEDM3)) ) { // led3 nchar=build_answer(txbuf, "ledm3"); e_ledm3_set_value(rxbuf+strlen(STR_SET_LEDM2), txbuf+nchar); b_report_pending=true; } else if( !strncmp((const char*)rxbuf, STR_SET_LEDM4, strlen(STR_SET_LEDM4)) ) { // led4 nchar=build_answer(txbuf, "ledm4"); e_ledm4_set_value(rxbuf+strlen(STR_SET_LEDM2), txbuf+nchar); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=accx") ) { // accelerometer nchar=build_answer(txbuf, "accx"); accelerometer_measure(0, txbuf+nchar); b_report_pending=true; } else if( !strcmp((const char*)rxbuf, "get_sensor_value sensor=accy") ) { // accelerometer nchar=build_answer(txbuf, "accy"); accelerometer_measure(1, txbuf+nchar); b_report_pending=true; } } if( b_report_pending && Is_usb_in_ready(EP_FC_IN) ) { U8 data_to_transfer; char* ptr_cram=txbuf; //printf( "Sending %s", txbuf); #if 0 Usb_reset_endpoint_fifo_access(EP_FC_IN); usb_write_ep_txpacket(EP_FC_IN, &txbuf, TXBUF_SIZE, NULL); Usb_ack_in_ready_send(EP_FC_IN); #endif data_to_transfer = strlen(txbuf); while (data_to_transfer) { while (!Is_usb_in_ready(EP_FC_IN)); Usb_reset_endpoint_fifo_access(EP_FC_IN); data_to_transfer = usb_write_ep_txpacket(EP_FC_IN, ptr_cram, data_to_transfer, (const void**)&ptr_cram); Usb_ack_in_ready_send(EP_FC_IN); } b_report_pending=false; } #ifdef FREERTOS_USED } #endif }
static void prvCheckTask( void *pvParameters ) { unsigned ulLastRegTest1Count = 0, ulLastRegTest2Count = 0; portTickType xLastExecutionTime; /* To prevent compiler warnings. */ ( void ) pvParameters; /* Initialise the variable used to control our iteration rate prior to its first use. */ xLastExecutionTime = xTaskGetTickCount(); for( ;; ) { /* Wait until it is time to run the tests again. */ vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_PERIOD ); /* Has an error been found in any task? */ if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x01UL; } if( xAreQueuePeekTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x02UL; } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { ulErrorCode |= 0x04UL; } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x20UL; } if( xArePollingQueuesStillRunning() != pdTRUE ) { ulErrorCode |= 0x40UL; } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x80UL; } if( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x100UL; } if( ulLastRegTest1Count == ulRegTest1Counter ) { ulErrorCode |= 0x200UL; } if( ulLastRegTest2Count == ulRegTest2Counter ) { ulErrorCode |= 0x200UL; } /* Remember the reg test counts so a stall in their values can be detected next time around. */ ulLastRegTest1Count = ulRegTest1Counter; ulLastRegTest2Count = ulRegTest2Counter; } }
void MPU6050Task(void) { char uart_out[32]; uint8_t controller_command = 0; uint8_t pre_command = 0; uint8_t count = 0; MPU6050_Task_Suspend(); vTaskDelayUntil(&xLastWakeTime, xFrequency); // wait while sensor is ready initKalman(&kalmanX); initKalman(&kalmanY); MPU6050_ReadAccelerometer(); accX = MPU6050_Data.Accelerometer_X; accY = MPU6050_Data.Accelerometer_Y; accZ = MPU6050_Data.Accelerometer_Z; float roll = atan2(-accY, -accZ) * RAD_TO_DEG; float pitch = atan(-accX / sqrt1(Square(accY) + Square(accZ))) * RAD_TO_DEG; setAngle(&kalmanX, roll); // Set starting angle setAngle(&kalmanY, pitch); while (1) { /* Read all data from sensor */ MPU6050_ReadAccGyo(); accX = MPU6050_Data.Accelerometer_X; accY = MPU6050_Data.Accelerometer_Y; accZ = MPU6050_Data.Accelerometer_Z; gyroX = MPU6050_Data.Gyroscope_X; gyroY = MPU6050_Data.Gyroscope_Y; gyroZ = MPU6050_Data.Gyroscope_Z; float roll = atan2(-accY, -accZ) * RAD_TO_DEG; float pitch = atan(-accX / sqrt1(Square(accY) + Square(accZ))) * RAD_TO_DEG; float gyroXrate = gyroX * MPU6050_Data.Gyro_Mult; // Convert to deg/s float gyroYrate = gyroY * MPU6050_Data.Gyro_Mult; // Convert to deg/s // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { setAngle(&kalmanX, roll); } else { kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter } if (Abs(kalAngleX) > 90) gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); #ifdef DEBUG USART1_puts("\r\n"); shell_float2str(accZ, uart_out); USART1_puts(uart_out); #else if (accY < -6300) { controller_command = 1; } else if (accY > 6300) { controller_command = 2; } else if (accZ < 0 && kalAngleY > 47) { controller_command = 3; } else if (accZ < 0 && kalAngleY < -30) { controller_command = 4; } else if (accZ < 0 && kalAngleY > 25 && kalAngleY < 47) { controller_command = 5; } else { controller_command = 6; } // check hand gesture for a while if (count == 5 && pre_command == controller_command) { switch (controller_command) { case 1: USART1_puts("\r\nmove right"); break; case 2: USART1_puts("\r\nmove left"); break; case 3: USART1_puts("\r\nforward"); break; case 4: USART1_puts("\r\nDOWN"); break; case 5: USART1_puts("\r\nUP"); break; case 6: USART1_puts("\r\nsuspend"); break; } } else if (pre_command == controller_command) { count++; } else { pre_command = controller_command; count = 0; } #endif vTaskDelayUntil(&xLastWakeTime, xFrequency); } }
static void vCheckTask( void *pvParameters ) { static unsigned long ulErrorDetected = pdFALSE; portTickType xLastExecutionTime; unsigned char *cErrorMessage = " FAIL"; unsigned char *cSuccessMessage = " PASS"; unsigned portBASE_TYPE uxColumn = mainMAX_WRITE_COLUMN; LCDMessage xMessage; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil() works correctly. */ xLastExecutionTime = xTaskGetTickCount(); for( ;; ) { /* Wait until it is time for the next cycle. */ vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_CYCLE_TIME ); /* Has an error been found in any of the standard demo tasks? */ if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { ulErrorDetected = pdTRUE; } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { ulErrorDetected = pdTRUE; } if( xAreBlockingQueuesStillRunning() != pdTRUE ) { ulErrorDetected = pdTRUE; } if( xAreComTestTasksStillRunning() != pdTRUE ) { ulErrorDetected = pdTRUE; } if( xAreDynamicPriorityTasksStillRunning() != pdTRUE ) { ulErrorDetected = pdTRUE; } /* Calculate the LCD line on which we would like the message to be displayed. The column variable is used for convenience as it is incremented each cycle anyway. */ xMessage.ucLine = ( unsigned char ) ( uxColumn & 0x01 ); /* The message displayed depends on whether an error was found or not. Any discovered error is latched. Here the column variable is used as an index into the text string as a simple way of moving the text from column to column. */ if( ulErrorDetected == pdFALSE ) { xMessage.pucString = cSuccessMessage + uxColumn; } else { xMessage.pucString = cErrorMessage + uxColumn; } /* Send the message to the print task for display. */ xQueueSend( xLCDQueue, ( void * ) &xMessage, mainNO_DELAY ); /* Make sure the message is printed in a different column the next time around. */ uxColumn--; if( uxColumn == 0 ) { uxColumn = mainMAX_WRITE_COLUMN; } } }
static void vErrorChecks( void *pvParameters ) { TickType_t xToggleRate = mainNO_ERROR_TOGGLE_PERIOD, xLastWakeTime; /* Ensure the parameter was passed in as expected. This is just a test of the kernel port, the parameter is not actually used for anything. The pointer will only actually be either 3 or 2 bytes, depending on the memory model. */ if( pvParameters != ( void * ) mainCHECK_PARAMETER_VALUE ) { xToggleRate = mainERROR_TOGGLE_PERIOD; } /* Initialise xLastWakeTime before it is used. After this point it is not written to directly. */ xLastWakeTime = xTaskGetTickCount(); /* Cycle for ever, delaying then checking all the other tasks are still operating without error. */ for( ;; ) { /* Wait until it is time to check all the other tasks again. */ vTaskDelayUntil( &xLastWakeTime, xToggleRate ); if( xAreDynamicPriorityTasksStillRunning() != pdTRUE ) { xToggleRate = mainERROR_TOGGLE_PERIOD; } if( sRegTestStatus != pdPASS ) { xToggleRate = mainERROR_TOGGLE_PERIOD; } #ifdef __IAR_78K0R_Kx3__ { /* Only the Kx3 runs all the tasks. */ if( xArePollingQueuesStillRunning() != pdTRUE) { xToggleRate = mainERROR_TOGGLE_PERIOD; } if( xAreSemaphoreTasksStillRunning() != pdTRUE) { xToggleRate = mainERROR_TOGGLE_PERIOD; } if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { xToggleRate = mainERROR_TOGGLE_PERIOD; } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { xToggleRate = mainERROR_TOGGLE_PERIOD; } } #endif /* Toggle the LED. The toggle rate will depend on whether or not an error has been found in any tasks. */ mainLED_0 = !mainLED_0; } }
void vCheckTask( void *pvParameters ) { unsigned long ulRow = 0; portTickType xDelay = 0; unsigned short usErrorCode = 0; unsigned long ulIteration = 0; extern unsigned portSHORT usMaxJitter; /* Intialise the sleeper. */ xDelay = xTaskGetTickCount(); for( ;; ) { /* Perform this check every mainCHECK_DELAY milliseconds. */ vTaskDelayUntil( &xDelay, mainCHECK_DELAY ); /* Check that all of the Demo tasks are still running. */ if( pdTRUE != xAreBlockingQueuesStillRunning() ) { usErrorCode |= 0x1; } if( pdTRUE != xAreBlockTimeTestTasksStillRunning() ) { usErrorCode |= 0x2; } if( pdTRUE != xAreCountingSemaphoreTasksStillRunning() ) { usErrorCode |= 0x4; } if( pdTRUE != xIsCreateTaskStillRunning() ) { usErrorCode |= 0x8; } if( pdTRUE != xAreDynamicPriorityTasksStillRunning() ) { usErrorCode |= 0x10; } if( pdTRUE != xAreMathsTaskStillRunning() ) { usErrorCode |= 0x20; } if( pdTRUE != xAreGenericQueueTasksStillRunning() ) { usErrorCode |= 0x40; } if( pdTRUE != xAreIntegerMathsTaskStillRunning() ) { usErrorCode |= 0x80; } if( pdTRUE != xArePollingQueuesStillRunning() ) { usErrorCode |= 0x100; } if( pdTRUE != xAreQueuePeekTasksStillRunning() ) { usErrorCode |= 0x200; } if( pdTRUE != xAreSemaphoreTasksStillRunning() ) { usErrorCode |= 0x400; } if( pdTRUE != xAreComTestTasksStillRunning() ) { usErrorCode |= 0x800; } if( pdTRUE != xAreIntQueueTasksStillRunning() ) { usErrorCode |= 0x1000; } /* Clear the display. */ LCD_Character_Display_ClearDisplay(); if( 0 == usErrorCode ) { LCD_Character_Display_Position( ( ulRow ) & 0x1, 0); LCD_Character_Display_PrintString( "Pass: "******"Jitter(ns):" ); LCD_Character_Display_PrintNumber( ( usMaxJitter * mainNS_PER_CLOCK ) ); } else { /* Do something to indicate the failure. */ LCD_Character_Display_Position( ( ulRow ) & 0x1, 0 ); LCD_Character_Display_PrintString( "Fail at: " ); LCD_Character_Display_PrintNumber( ulIteration ); LCD_Character_Display_Position( ( ++ulRow ) & 0x1, 0 ); LCD_Character_Display_PrintString( "Error: 0x" ); LCD_Character_Display_PrintHexUint16( usErrorCode ); } } }
static void stabilizerTask(void* param) { uint32_t attitudeCounter = 0; uint32_t altHoldCounter = 0; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz // Magnetometer not yet used more then for logging. imu9Read(&gyro, &acc, &mag); if (imu6IsCalibrated()) { commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); // 250HZ if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z); accMAG = (acc.x*acc.x) + (acc.y*acc.y) + (acc.z*acc.z); // Estimate speed from acc (drifts) vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT; controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; /* Call out after performing attitude updates, if any functions would like to use the calculated values. */ stabilizerPostAttitudeUpdateCallOut(); } // 100HZ if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER)) { stabilizerAltHoldUpdate(); altHoldCounter = 0; } if (rollType == RATE) { rollRateDesired = eulerRollDesired; } if (pitchType == RATE) { pitchRateDesired = eulerPitchDesired; } if (yawType == RATE) { yawRateDesired = -eulerYawDesired; } // TODO: Investigate possibility to subtract gyro drift. controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired, yawRateDesired); controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); if (!altHold || !imuHasBarometer()) { // Use thrust from controller if not in altitude hold mode commanderGetThrust(&actuatorThrust); } else { // Added so thrust can be set to 0 while in altitude hold mode after disconnect commanderWatchdog(); } /* Call out before performing thrust updates, if any functions would like to influence the thrust. */ stabilizerPreThrustUpdateCallOut(); if (actuatorThrust > 0) { #if defined(TUNE_ROLL) distributePower(actuatorThrust, actuatorRoll, 0, 0); #elif defined(TUNE_PITCH) distributePower(actuatorThrust, 0, actuatorPitch, 0); #elif defined(TUNE_YAW) distributePower(actuatorThrust, 0, 0, -actuatorYaw); #else distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw); #endif } else { distributePower(0, 0, 0, 0); controllerResetAllPID(); } } } }
void task_stopcharge(void * pvParameters) { static int num=0; //read_meter_ALL float stop_valueL=0; float stop_valueR=0; float basic_valueL=0; float basic_valueR=0; portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); P1R(1); P2R(1); meter_init(); #ifdef debug printf("meter\r\n"); #endif if(!read_meter_ALL(meter_num_L,1000)) { PORTL.status=broken; P1Y(1); refresh_win0(); printf("first read left meter error\r\n"); } if(!read_meter_ALL(meter_num_R,1000)) { PORTR.status=broken; P2Y(1); refresh_win0(); printf("first read right meter error\r\n"); } while(1) { num++; if(isstop==stopL.stop) { P1GN; if(!read_meter_ALL(meter_num_L,500)) { //充电过程中电表读取失败的处理,下同 PORTL.status=broken; P1Y(1); P1G(0); close_port(&PORTL.port); stopL.stop=stopnone; refresh_win0(); goto Broken; #ifdef debug printf("read meter left error\r\n"); #endif } if(isbasic==stopL.basic) { if(electric==stopL.charge_way) { basic_valueL=KWHL; stop_valueL=KWHL+stopL.value; #ifdef debug printf("stop_valueL:%f",stop_valueL); #endif } if(money==stopL.charge_way) { basic_valueL=KWHL; stop_valueL=KWHL+(float)stopL.value/price; #ifdef debug printf("stop_valueL:%f\r\n",stop_valueL); #endif } if(automate==stopL.charge_way) { basic_valueL=KWHL; #ifdef debug printf("aautomate:%f\r\n",stop_valueL); #endif } // get_ele_data(portL); stopL.basic=nonebasic; } useL.value=KWHL-basic_valueL; useL.money=useL.value*price; // printf("valueL:%f",useL.value); useL.value*=10; useL.money*=10; if((stopL.charge_way==money)||(stopL.charge_way==electric)) { if(KWHL>=stop_valueL) { close_port(&stopL.port); PORTL.status=available; stop_valueL=0; refresh_win0(); memset(&useL,0,sizeof(useinfo)); memset(&stopL,0,sizeof(stop_charging)); } } if(stopL.charge_way==automate) { if(electricityL<=threshold) { //电流低于阈值时停止充电 close_port(&stopL.port); PORTL.status=available; refresh_win0(); memset(&useL,0,sizeof(useinfo)); memset(&stopL,0,sizeof(stop_charging)); } } } if(isstop==stopR.stop) { P2GN; if(!read_meter_ALL(meter_num_R,500)) { close_port(&PORTR.port); PORTR.status=broken; P2Y(1); P2G(0); stopR.stop=stopnone; refresh_win0(); goto Broken; #ifdef debug printf("read meter rigth error\r\n"); #endif } if(isbasic==stopR.basic) { if(electric==stopR.charge_way) { basic_valueR=KWHR; stop_valueR=KWHR+stopR.value; #ifdef debug printf("stop_valueR:%f\r\n",stop_valueR); #endif } if(money==stopR.charge_way) { basic_valueR=KWHR; stop_valueR=KWHR+(float)stopR.value/price; #ifdef debug printf("stop_valueR:%f\r\n",stop_valueR); #endif } if(automate==stopR.charge_way) { basic_valueR=KWHR; #ifdef debug printf("aautomate:%f\r\n",stop_valueR); #endif } // get_ele_data(portR); stopR.basic=nonebasic; } useR.value=KWHR-basic_valueR; useR.money=useR.value*price; // printf("valueR:%f",useR.value); useR.value*=10; useR.money*=10; if((stopR.charge_way==money)||(stopR.charge_way==electric)) { if(KWHR>=stop_valueR) { close_port(&stopR.port); PORTR.status=available; stop_valueR=0; refresh_win0(); memset(&useR,0,sizeof(useinfo)); memset(&stopR,0,sizeof(stop_charging)); #ifdef debug printf("dianliang jine\r\n"); #endif } } if(stopR.charge_way==automate) { if(electricityR<=threshold) { //电流低于阈值时停止充电 close_port(&stopR.port); PORTR.status=available; stop_valueR=0; refresh_win0(); memset(&useR,0,sizeof(useinfo)); memset(&stopR,0,sizeof(stop_charging)); #ifdef debug printf("auto\r\n"); #endif } } } Broken: if(broken==PORTL.status) { if(read_meter_ALL(meter_num_L,300)) { PORTL.status=available; P1Y(0); refresh_win0(); } } if(broken==PORTR.status) { if(read_meter_ALL(meter_num_R,300)) { PORTR.status=available; P2Y(0); refresh_win0(); } } vTaskDelayUntil( &xLastWakeTime, ( 100 / portTICK_RATE_MS ) ); } }
static void prvCheckTask( void * pvParameters ) { portTickType xNextWakeTime, xPeriod = mainNO_ERROR_PERIOD; static volatile unsigned long ulErrorCode = 0UL; /* Just to remove the compiler warning. */ ( void ) pvParameters; /* Initialise xNextWakeTime prior to its first use. From this point on the value of the variable is handled automatically by the kernel. */ xNextWakeTime = xTaskGetTickCount(); for( ;; ) { /* Delay until it is time for this task to execute again. */ vTaskDelayUntil( &xNextWakeTime, xPeriod ); /* Check all the other tasks in the system - latch any reported errors into the ulErrorCode variable. */ if( xAreBlockingQueuesStillRunning() != pdTRUE ) { ulErrorCode |= 0x01UL; } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x02UL; } if( xAreCountingSemaphoreTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x04UL; } if( xIsCreateTaskStillRunning() != pdTRUE ) { ulErrorCode |= 0x08UL; } if( xAreDynamicPriorityTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x10UL; } if( xAreGenericQueueTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x20UL; } if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { ulErrorCode |= 0x40UL; } if( xArePollingQueuesStillRunning() != pdTRUE ) { ulErrorCode |= 0x80UL; } if( xAreQueuePeekTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x100UL; } if( xAreRecursiveMutexTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x200UL; } if( xAreSemaphoreTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x400UL; } if( xAreComTestTasksStillRunning() != pdTRUE ) { ulErrorCode |= 0x800UL; } /* Reduce the block period and in so doing increase the frequency at which this task executes if any errors have been latched. The increased frequency causes the LED toggle rate to increase and so gives some visual feedback that an error has occurred. */ if( ulErrorCode != 0x00 ) { xPeriod = mainERROR_PERIOD; } /* Finally toggle the LED. */ vParTestToggleLED( LED_POWER ); } }
//***************************************************************************** // // This task reads the buttons' state and passes this information to LEDTask. // //***************************************************************************** static void SwitchTask(void *pvParameters) { portTickType ui16LastTime; uint32_t ui32SwitchDelay = 25; uint8_t ui8CurButtonState, ui8PrevButtonState; uint8_t ui8Message; ui8CurButtonState = ui8PrevButtonState = 0; // // Get the current tick count. // ui16LastTime = xTaskGetTickCount(); // // Loop forever. // while(1) { // // Poll the debounced state of the buttons. // ui8CurButtonState = ButtonsPoll(0, 0); // // Check if previous debounced state is equal to the current state. // if(ui8CurButtonState != ui8PrevButtonState) { ui8PrevButtonState = ui8CurButtonState; // // Check to make sure the change in state is due to button press // and not due to button release. // if((ui8CurButtonState & ALL_BUTTONS) != 0) { if((ui8CurButtonState & ALL_BUTTONS) == LEFT_BUTTON) { ui8Message = LEFT_BUTTON; } else if((ui8CurButtonState & ALL_BUTTONS) == RIGHT_BUTTON) { ui8Message = RIGHT_BUTTON; } // // Pass the value of the button pressed to LEDTask. // if(xQueueSend(g_pLEDQueue, &ui8Message, portMAX_DELAY) != pdPASS) { // // Error. The queue should never be full. If so print the // error message on UART and wait for ever. // UARTprintf("\nQueue full. This should never happen.\n"); while(1) { } } } } // // Wait for the required amount of time to check back. // vTaskDelayUntil(&ui16LastTime, ui32SwitchDelay / portTICK_RATE_MS); } }
/** * Module thread, should not return. */ static void altitudeTask(void *parameters) { BaroAltitudeData data; portTickType lastSysTime; #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeData sonardata; int32_t value=0,timeout=5; float coeff=0.25,height_out=0,height_in=0; PIOS_HCSR04_Init(); PIOS_HCSR04_Trigger(); #endif PIOS_BMP085_Init(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { #if defined(PIOS_INCLUDE_HCSR04) // Compute the current altitude (all pressures in kPa) if(PIOS_HCSR04_Completed()) { value = PIOS_HCSR04_Get(); if((value>100) && (value < 15000)) //from 3.4cm to 5.1m { height_in = value*0.00034; height_out = (height_out * (1 - coeff)) + (height_in * coeff); sonardata.Altitude = height_out; // m/us } // Update the AltitudeActual UAVObject SonarAltitudeSet(&sonardata); timeout=5; PIOS_HCSR04_Trigger(); } if(timeout--) { //retrigger timeout=5; PIOS_HCSR04_Trigger(); } #endif // Update the temperature data PIOS_BMP085_StartADC(TemperatureConv); #ifdef PIOS_BMP085_HAS_GPIOS xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else vTaskDelay(5 / portTICK_RATE_MS); #endif PIOS_BMP085_ReadADC(); alt_ds_temp += PIOS_BMP085_GetTemperature(); // Update the pressure data PIOS_BMP085_StartADC(PressureConv); #ifdef PIOS_BMP085_HAS_GPIOS xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); #else vTaskDelay(26 / portTICK_RATE_MS); #endif PIOS_BMP085_ReadADC(); alt_ds_pres += PIOS_BMP085_GetPressure(); if (++alt_ds_count >= alt_ds_size) { alt_ds_count = 0; // Convert from 1/10ths of degC to degC data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); alt_ds_temp = 0; // Convert from Pa to kPa data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); alt_ds_pres = 0; // Compute the current altitude (all pressures in kPa) data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); // Update the AltitudeActual UAVObject BaroAltitudeSet(&data); } // Delay until it is time to read the next sample vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); } }