Пример #1
0
void vuIP_Task( void *pvParameters )
{
portBASE_TYPE i;
unsigned long ulNewEvent = 0UL;
unsigned long ulUIP_Events = 0UL;

	( void ) pvParameters;
	
	/* Initialise the uIP stack. */
	prvInitialise_uIP();

	/* Initialise the MAC. */
	vInitEmac();

	while( lEMACWaitForLink() != pdPASS )
    {
        vTaskDelay( uipINIT_WAIT );
    }

	for( ;; )
	{
		if( ( ulUIP_Events & uipETHERNET_RX_EVENT ) != 0UL )
		{		
			/* Is there received data ready to be processed? */
			uip_len = ( unsigned short ) ulEMACRead();
			
			if( ( uip_len > 0 ) && ( uip_buf != NULL ) )
			{
				/* Standard uIP loop taken from the uIP manual. */
				if( xHeader->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();
						vEMACWrite();
					}
				}
				else if( xHeader->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 )
					{
						vEMACWrite();
					}
				}
			}
			else
			{
				ulUIP_Events &= ~uipETHERNET_RX_EVENT;
			}
		}
		
		if( ( ulUIP_Events & uipPERIODIC_TIMER_EVENT ) != 0UL )
		{
			ulUIP_Events &= ~uipPERIODIC_TIMER_EVENT;
					
			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();
					vEMACWrite();
				}
			}
		}
		
		/* Call the ARP timer function every 10 seconds. */
		if( ( ulUIP_Events & uipARP_TIMER_EVENT ) != 0 )
		{
			ulUIP_Events &= ~uipARP_TIMER_EVENT;
			uip_arp_timer();
		}
			
		if( ulUIP_Events == pdFALSE )
		{
			xQueueReceive( xEMACEventQueue, &ulNewEvent, portMAX_DELAY );
			ulUIP_Events |= ulNewEvent;
		}
	}
}
/* -------------------------------------------------------------------- */
void mainTask(void *p) {
	
	// packet used to handle incoming communication
	csp_packet_t * outcomingPacket = csp_buffer_get(CSP_PACKET_SIZE);

	// character received from uart
	char inChar;
	
	/* The variable used to receive from the queue. */
	xCSPStackEvent_t xReceivedEvent;
	
	int i; // iterator
	
	// text buffer for uart
	char text[20];
	
	// infinite while loop of the program 
	while (1) {
		
		if (xQueueReceive(xCSPEventQueue, &xReceivedEvent, 1)) {
			
			switch (xReceivedEvent.eEventType) {
			
				case generalCommEvent:
			
					// send its content to the uart
					for (i = 0; i < ((csp_packet_t *) xReceivedEvent.pvData)->length; i++) {
						usartBufferPutByte(pc_usart_buffer, ((csp_packet_t *) xReceivedEvent.pvData)->data[i], 10);
					}
			
				break;
				case pingReceivedEvent:
			
					// calculate the ping return time
					if ((int16_t) milisecondsTimer - (int16_t) pingSent > 0)
						timediff = milisecondsTimer - pingSent;
					else
						timediff = (int16_t) milisecondsTimer - (int16_t) pingSent + (int16_t) 1000;

					itoa(timediff, text, 10);
					usartBufferPutString(pc_usart_buffer, "ping received in ", 10);
					usartBufferPutString(pc_usart_buffer, text, 10);
					usartBufferPutString(pc_usart_buffer, "ms\n\r", 10);
			
				break;
			
				default:
					/* Should not get here. */
				break;
			}
		}
		
		// if there is something from the uart
		if (usartBufferGetByte(pc_usart_buffer, &inChar, 0)) {
		
			outcomingPacket->data[0] = inChar;
			outcomingPacket->length = 1;
		
			switch (inChar) {
			
				// ask board for free memory heap
				case 'm':
					csp_sendto(CSP_PRIO_NORM, CSP_BOARD_ADDRESS, 16, 15, CSP_O_NONE, outcomingPacket, 10);
				break;
			
				// ask board for status
				case 'h':
					csp_sendto(CSP_PRIO_NORM, CSP_BOARD_ADDRESS, 17, 15, CSP_O_NONE,  outcomingPacket, 10);
				break;
				
				// ask board for status
				case 'a':
				csp_sendto(CSP_PRIO_NORM, CSP_BOARD_ADDRESS, 18, 15, CSP_O_NONE,  outcomingPacket, 10);
				break;
			
				// ask board for status
				case 'p':	
					outcomingPacket->data[0] = 0;
					outcomingPacket->length = 1;
					pingSent = milisecondsTimer;
					csp_sendto(CSP_PRIO_NORM, CSP_BOARD_ADDRESS, 1, 32, CSP_O_NONE, outcomingPacket, 10);
				break;
			
				// sends the char and is supposed to receive it back
				default:
					csp_sendto(CSP_PRIO_NORM, CSP_BOARD_ADDRESS, 15, 15, CSP_O_NONE,  outcomingPacket, 10);
				break;
			}
		}
	}
}
Пример #3
0
BaseType_t readFromNFCtxQ(char* msg) {
    return xQueueReceive(nfc_appData.txBufferQ, msg, portMAX_DELAY);
}
/**
 * \brief UART task
 *
 * This task runs in the background to handle the queued, incoming terminal
 * characters and write them to the terminal text buffer. It does not print
 * anything to the display -- that is done by \ref terminal_task().
 *
 * \param params Parameters for the task. (Not used.)
 */
static void uart_task(void *params)
{
	uint8_t *current_line_ptr;
	uint8_t *current_char_ptr;
	uint8_t current_column = 0;

	for (;;) {
		// Show that task is executing
		oled1_set_led_state(&oled1, OLED1_LED1_ID, true);

		// Grab terminal mutex
		xSemaphoreTake(terminal_mutex, portMAX_DELAY);

		current_line_ptr = terminal_buffer[terminal_line_offset];
		current_char_ptr = current_line_ptr + current_column;

		// Any characters queued? Handle them!
		while (xQueueReceive(terminal_in_queue, current_char_ptr, 0)) {
			/* Newline-handling is difficult because all terminal emulators
			 * seem to do it their own way. The method below seems to work
			 * with Putty and Realterm out of the box.
			 */
			switch (*current_char_ptr) {
			case '\r':
				// Replace \r with \0 and move head to next line
				*current_char_ptr = '\0';

				current_column = 0;
				terminal_line_offset = (terminal_line_offset + 1)
						% TERMINAL_BUFFER_LINES;
				current_line_ptr = terminal_buffer[terminal_line_offset];
				current_char_ptr = current_line_ptr + current_column;
				break;

			case '\n':
				// For \n, do nothing -- it is replaced with \0 later
				break;

			default:
				// For all other characters, just move head to next char
				current_column++;
				if (current_column >= TERMINAL_COLUMNS) {
					current_column = 0;
					terminal_line_offset = (terminal_line_offset + 1)
							% TERMINAL_BUFFER_LINES;
					current_line_ptr = terminal_buffer[terminal_line_offset];
				}
				current_char_ptr = current_line_ptr + current_column;
			}

			// Set zero-terminator at head
			*current_char_ptr = '\0';
		}

		xSemaphoreGive(terminal_mutex);

		oled1_set_led_state(&oled1, OLED1_LED1_ID, false);

		vTaskDelay(UART_TASK_DELAY);
	}
}
Пример #5
0
void vuIP_Task( void *pvParameters )
{
portBASE_TYPE i;
unsigned long ulNewEvent = 0UL, ulUIP_Events = 0UL;
long lPacketLength;

	/* Just to prevent compiler warnings about the unused parameter. */
	( void ) pvParameters;

	/* Initialise the uIP stack, configuring for web server usage. */
	prvInitialise_uIP();

	/* Initialise the MAC and PHY. */
	prvInitEmac();

	for( ;; )
	{
		/* Is there received data ready to be processed? */
		lPacketLength = MSS_MAC_rx_packet();

		/* Statements to be executed if data has been received on the Ethernet. */
		if( ( lPacketLength > 0 ) && ( uip_buf != NULL ) )
		{
			uip_len = ( u16_t ) lPacketLength;
			
			/* Standard uIP loop taken from the uIP manual. */
			if( xHeader->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();
					vEMACWrite();
				}
			}
			else if( xHeader->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 )
				{
					vEMACWrite();
				}
			}
		}
		else
		{
			/* Clear the RX event latched in ulUIP_Events - if one was latched. */
			ulUIP_Events &= ~uipETHERNET_RX_EVENT;
		}

		/* Statements to be executed if the TCP/IP period timer has expired. */
		if( ( ulUIP_Events & uipPERIODIC_TIMER_EVENT ) != 0UL )
		{
			ulUIP_Events &= ~uipPERIODIC_TIMER_EVENT;

			if( uip_buf != NULL )
			{
				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();
						vEMACWrite();
					}
				}
			}
		}

		/* Statements to be executed if the ARP timer has expired. */
		if( ( ulUIP_Events & uipARP_TIMER_EVENT ) != 0 )
		{
			ulUIP_Events &= ~uipARP_TIMER_EVENT;
			uip_arp_timer();
		}

		/* If all latched events have been cleared - block until another event
		occurs. */
		if( ulUIP_Events == pdFALSE )
		{
			xQueueReceive( xEMACEventQueue, &ulNewEvent, portMAX_DELAY );
			ulUIP_Events |= ulNewEvent;
		}
	}
}
Пример #6
0
static void prvLCDTaskLine2( void *pvParameters )
{
struct _LCD_Params *pxLCDParamaters = ( struct _LCD_Params * ) pvParameters;
unsigned short usPosition = 0U;
unsigned char ucDirection = lcdRIGHT_TO_LEFT, ucStatus = lcdRUNNING, ucQueueData;
portTickType xDelayTicks = ( pxLCDParamaters->Speed / portTICK_RATE_MS );
	
	for(;;)
	{
		/* Wait for a message from an IRQ handler. */
		if( xQueueReceive( xButtonCommandQueue, &ucQueueData, xDelayTicks ) != pdPASS )
		{
			/* A message was not received before xDelayTicks ticks passed, so
			generate the next string to display and display it. */
			prvDisplayNextString( pxLCDParamaters->Line, &( pxLCDParamaters->ptr_str[ usPosition ] ) );
			
			/* Move the string in whichever direction the scroll is currently 
			going in. */
			prvScrollString( &ucDirection, &usPosition, strlen( pxLCDParamaters->ptr_str ) );			
		}
		else
		{
			/* A command was received.  Process it. */
			switch( ucQueueData )
			{
				case lcdSTART_STOP_COMMAND :

					/* If the LCD is running, top it.  If the LCD is stopped, start
					it. */
					ucStatus = !ucStatus;
					
					if( ucStatus == lcdRUNNING )
					{
						xDelayTicks = ( pxLCDParamaters->Speed / portTICK_RATE_MS );
					}
					else
					{
						xDelayTicks = portMAX_DELAY;
					}
					break;

					
				case lcdSHIFT_BACK_COMMAND :

					if( ucStatus != lcdRUNNING )
					{
						/* If not already at the start of the display.... */
						if( usPosition != 0U )
						{
							/* ....move the display position back by one char. */
							usPosition--;												
							prvDisplayNextString( pxLCDParamaters->Line, &( pxLCDParamaters->ptr_str[ usPosition ] ) );
						}
					}
					break;
				
				
				case lcdSHIFT_FORWARD_COMMAND :

					if( ucStatus != lcdRUNNING )
					{
						/* If not already at the end of the display.... */
						if( usPosition != ( strlen( pxLCDParamaters->ptr_str ) - ( lcdSTRING_LEN - 1 ) ) )
						{
							/* ....move the display position forward by one 
							char. */
							usPosition++;
							prvDisplayNextString( pxLCDParamaters->Line, &( pxLCDParamaters->ptr_str[ usPosition ] ) );
						}
					}
					break;
			}
		}
	}
}
Пример #7
0
static void prvLowPriorityPeekTask( void *pvParameters )
{
xQueueHandle xQueue = ( xQueueHandle ) pvParameters;
unsigned portLONG ulValue;

	for( ;; )
	{
		/* Write some data to the queue.  This should unblock the highest 
		priority task that is waiting to peek data from the queue. */
		ulValue = 0x11223344;
		if( xQueueSendToBack( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
		{
			/* We were expecting the queue to be empty so we should not of
			had a problem writing to the queue. */
			xErrorDetected = pdTRUE;
		}

		/* By the time we get here the data should have been removed from
		the queue. */
		if( uxQueueMessagesWaiting( xQueue ) != 0 )
		{
			xErrorDetected = pdTRUE;
		}

		/* Write another value to the queue, again waking the highest priority
		task that is blocked on the queue. */
		ulValue = 0x01234567;
		if( xQueueSendToBack( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
		{
			/* We were expecting the queue to be empty so we should not of
			had a problem writing to the queue. */
			xErrorDetected = pdTRUE;
		}

		/* All the other tasks should now have successfully peeked the data.
		The data is still in the queue so we should be able to receive it. */
		ulValue = 0;
		if( xQueueReceive( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
		{
			/* We expected to receive the data. */
			xErrorDetected = pdTRUE;
		}

		if( ulValue != 0x01234567 )
		{
			/* We did not receive the expected value. */
		}
		
		/* Lets just delay a while as this is an intensive test as we don't
		want to starve other tests of processing time. */
		vTaskDelay( qpeekSHORT_DELAY );

		/* Unsuspend the other tasks so we can repeat the test - this time
		however not all the other tasks will peek the data as the high
		priority task is actually going to remove it from the queue.  Send
		to front is used just to be different.  As the queue is empty it
		makes no difference to the result. */
		vTaskResume( xMediumPriorityTask );
		vTaskResume( xHighPriorityTask );
		vTaskResume( xHighestPriorityTask );

		ulValue = 0xaabbaabb;
		if( xQueueSendToFront( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
		{
			/* We were expecting the queue to be empty so we should not of
			had a problem writing to the queue. */
			xErrorDetected = pdTRUE;
		}

		/* This time we should find that the queue is empty.  The high priority
		task actually removed the data rather than just peeking it. */
		if( xQueuePeek( xQueue, &ulValue, qpeekNO_BLOCK ) != errQUEUE_EMPTY )
		{
			/* We expected to receive the data. */
			xErrorDetected = pdTRUE;
		}

		/* Unsuspend the highest and high priority tasks so we can go back
		and repeat the whole thing.  The medium priority task should not be
		suspended as it was not able to peek the data in this last case. */
		vTaskResume( xHighPriorityTask );
		vTaskResume( xHighestPriorityTask );		

		/* Lets just delay a while as this is an intensive test as we don't
		want to starve other tests of processing time. */
		vTaskDelay( qpeekSHORT_DELAY );
	}
}
Пример #8
0
void mapRoverTask( void *param ){
//static portTASK_FUNCTION( mapRoverTask, pvParameters )
	RoverMapStruct *roverMapStruct = (RoverMapStruct *) param;
	
	MapCorner receivedCorner;

	double totalAngle = 0;
	double totalCalcAngle = 90.0;
	double area;
	
	char buf[20];
	double number;
	//int intPart, decimalPart;
	uint8_t cornersCount = 0;

	sprintf(guiMapCoordinates, "");
	polyComp = 0;
	
	for(;;){
		if (xQueueReceive(roverMapStruct->inQ, (void *) &receivedCorner, portMAX_DELAY) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		if(strlen(guiMapCoordinates) >= (BUFFER_SIZE - 10)){
			sprintf(guiMapCoordinates, "");
		}
		if(strlen(debugBuf) >= (BUFFER_SIZE - 10)){
			sprintf(debugBuf, "");
		}
		printFloat("QReced:", receivedCorner.distSide, 1);
		// Print the received distance reading
		vtLEDToggle(0x40);
		printFloat("Angle: ", receivedCorner.angleCornerExterior, 0);;
		double ang;
		if(roverMapStruct->taskFlags == REGULAR && roverMapStruct->numberSides != 0){
			receivedCorner.angleCornerExterior = 360.0/roverMapStruct->numberSides;
		}
		else{
			ang = receivedCorner.angleCornerExterior;
			receivedCorner.angleCornerExterior = receivedCorner.angleCornerExterior + receivedCorner.tempBefore/2.0;
			if(receivedCorner.angleCornerExterior>90) receivedCorner.angleCornerExterior = 90;
		}
		//printFloat("Side Dist: ", receivedCorner.distSide, 1);
		receivedCorner.tempPow = mapCorners[cornersCount-1].distFromSide * pow(receivedCorner.angleCornerExterior/90.0, 5);
		
		totalAngle += receivedCorner.angleCornerExterior;
		if(cornersCount != 0){
			//totalAngle += receivedCorner.angleCornerExterior;
			//receivedCorner.distSide += mapCorners[cornersCount-1].distFromSide;
			printFloat("PrevSide:", mapCorners[cornersCount-1].distFromSide, 1);
			printFloat("WithPrevSide:", receivedCorner.distSide, 1);
			
			// Teja experimenting
			receivedCorner.distSide +=  receivedCorner.tempFrontDist + mapCorners[cornersCount-1].distFromSide*sin(mapCorners[cornersCount-1].angleCornerExterior*M_PI/180.0)*sin(mapCorners[cornersCount-1].angleCornerExterior*M_PI/180.0);

			// Yasir's conversion to coordinates
			xPoints[cornersCount] = xPoints[cornersCount - 1] + receivedCorner.distSide*cos(totalCalcAngle*M_PI/180.0);
			yPoints[cornersCount] = yPoints[cornersCount - 1] + receivedCorner.distSide*sin(totalCalcAngle*M_PI/180.0);
			totalCalcAngle -= receivedCorner.angleCornerExterior;

			sprintf(buf, "C=%d Deg=%.2f Dist=%.2f A=%.2f\n", cornersCount+1, receivedCorner.angleCornerExterior, receivedCorner.distSide, calculateArea(cornersCount+1, xPoints, yPoints));
			strcat(debugBuf, buf);

			//sprintf(buf, "Deg=%.1f F=%.1f OA=%.1f SB=%.1f SD=%.1f SDP=%1.f FD=%.1f Dist=%.1f S=%d\n", receivedCorner.angleCornerExterior,receivedCorner.tempFront, ang, receivedCorner.tempBefore, mapCorners[cornersCount-1].distFromSide, receivedCorner.tempPow, receivedCorner.tempFrontDist, receivedCorner.distSide, cornersCount);
			//strcat(debugBuf, buf);

			//sprintf(buf, "A=%f\n", calculateArea(cornersCount+1, xPoints, yPoints));
			//strcat(debugBuf, buf);
			if ((totalAngle) >= TOTAL_ANGLE_THRESHOLD){
				// TODO: calculate area;
				// param for calculateArea (side) is 1 minus the number of sides
				if(polyComp == 0){
					polyComp = -1;
					sprintf(buf, "%d,%d ", (int)(xPoints[cornersCount]*MAP_SCALE_FACTOR + MAP_X_OFFSET), (int)(yPoints[cornersCount]*-MAP_SCALE_FACTOR + MAP_Y_OFFSET));
					strcat(guiMapCoordinates, buf);
				}
				else{
					polyComp = cornersCount;
					sprintf(buf, "%d,%d", MAP_X_OFFSET, MAP_Y_OFFSET);
					strcat(guiMapCoordinates, buf);
				}
				sprintf(buf, "\n** Polygon complete ** Corners=%d A=%f\n\n", cornersCount+1,calculateArea(cornersCount+1, xPoints, yPoints));
				strcat(debugBuf, buf);
			}
			else{
				sprintf(buf, "%d,%d ", (int)(xPoints[cornersCount]*MAP_SCALE_FACTOR + MAP_X_OFFSET), (int)(yPoints[cornersCount]*-MAP_SCALE_FACTOR + MAP_Y_OFFSET));
				strcat(guiMapCoordinates, buf);
			}
		}
		else{
			totalCalcAngle = 90;
			xPoints[0] = 0;
			xPoints[0] = 0;
			sprintf(buf, "%d,%d ", (int)(xPoints[cornersCount]*MAP_SCALE_FACTOR + MAP_X_OFFSET), (int)(yPoints[cornersCount]*-MAP_SCALE_FACTOR + MAP_Y_OFFSET));
			strcat(guiMapCoordinates, buf);
		}

		if(cornersCount < MAXIMUM_CORNERS){
			mapCorners[cornersCount] = receivedCorner;
			++cornersCount;
		}


		
	}
}
int os_queue_take(os_queue_t queue, void* item, system_tick_t delay, void*)
{
    return xQueueReceive(queue, item, delay)!=pdTRUE;
}
Пример #10
0
static void prvQueueSetReceivingTask( void *pvParameters )
{
uint32_t ulReceived;
QueueHandle_t xActivatedQueue;
TickType_t xBlockTime;

	/* Remove compiler warnings. */
	( void ) pvParameters;

	/* Create the queues and add them to the queue set before resuming the Tx
	task. */
	prvSetupTest();

	for( ;; )
	{
		/* For test coverage reasons, the block time is dependent on the
		priority of this task - which changes during the test.  When the task
		is at the idle priority it polls the queue set. */
		if( uxTaskPriorityGet( NULL ) == tskIDLE_PRIORITY )
		{
			xBlockTime = 0;
		}
		else
		{
			xBlockTime = portMAX_DELAY;
		}

		/* Wait for a message to arrive on one of the queues in the set. */
		xActivatedQueue = xQueueSelectFromSet( xQueueSet, portMAX_DELAY );		

		if( xActivatedQueue == NULL )
		{
			if( xBlockTime != 0 )
			{
				/* This should not happen as an infinite delay was used. */
				xQueueSetTasksStatus = pdFAIL;
			}
		}
		else
		{
			/* Reading from the queue should pass with a zero block time as
			this task will only run when something has been posted to a task
			in the queue set. */
			if( xQueueReceive( xActivatedQueue, &ulReceived, queuesetDONT_BLOCK ) != pdPASS )
			{
				xQueueSetTasksStatus = pdFAIL;
			}

			/* Ensure the value received was the value expected.  This function
			manipulates file scope data and is also called from an ISR, hence
			the critical section. */
			taskENTER_CRITICAL();
			{
				prvCheckReceivedValue( ulReceived );
			}
			taskEXIT_CRITICAL();

			if( xQueueSetTasksStatus == pdPASS )
			{
				ulCycleCounter++;
			}
		}
	}
}
Пример #11
0
static void prvSetupTest( void )
{
BaseType_t x;
uint32_t ulValueToSend = 0;

	/* Ensure the queues are created and the queue set configured before the
	sending task is unsuspended.

	First Create the queue set such that it will be able to hold a message for
	every space in every queue in the set. */
	xQueueSet = xQueueCreateSet( queuesetNUM_QUEUES_IN_SET * queuesetQUEUE_LENGTH );

	for( x = 0; x < queuesetNUM_QUEUES_IN_SET; x++ )
	{
		/* Create the queue and add it to the set.  The queue is just holding
		uint32_t value. */
		xQueues[ x ] = xQueueCreate( queuesetQUEUE_LENGTH, sizeof( uint32_t ) );
		configASSERT( xQueues[ x ] );
		if( xQueueAddToSet( xQueues[ x ], xQueueSet ) != pdPASS )
		{
			xQueueSetTasksStatus = pdFAIL;
		}
		else
		{
			/* The queue has now been added to the queue set and cannot be added to
			another. */
			if( xQueueAddToSet( xQueues[ x ], xQueueSet ) != pdFAIL )
			{
				xQueueSetTasksStatus = pdFAIL;
			}
		}
	}

	/* Attempt to remove a queue from a queue set it does not belong
	to (NULL being passed as the queue set in this case). */
	if( xQueueRemoveFromSet( xQueues[ 0 ], NULL ) != pdFAIL )
	{
		/* It is not possible to successfully remove a queue from a queue
		set it does not belong to. */
		xQueueSetTasksStatus = pdFAIL;
	}

	/* Attempt to remove a queue from the queue set it does belong to. */
	if( xQueueRemoveFromSet( xQueues[ 0 ], xQueueSet ) != pdPASS )
	{
		/* It should be possible to remove the queue from the queue set it
		does belong to. */
		xQueueSetTasksStatus = pdFAIL;
	}

	/* Add an item to the queue before attempting to add it back into the
	set. */
	xQueueSend( xQueues[ 0 ], ( void * ) &ulValueToSend, 0 );
	if( xQueueAddToSet( xQueues[ 0 ], xQueueSet ) != pdFAIL )
	{
		/* Should not be able to add a non-empty queue to a set. */
		xQueueSetTasksStatus = pdFAIL;
	}

	/* Remove the item from the queue before adding the queue back into the
	set so the dynamic tests can begin. */
	xQueueReceive( xQueues[ 0 ], &ulValueToSend, 0 );
	if( xQueueAddToSet( xQueues[ 0 ], xQueueSet ) != pdPASS )
	{
		/* If the queue was successfully removed from the queue set then it
		should be possible to add it back in again. */
		xQueueSetTasksStatus = pdFAIL;
	}

	/* The task that sends to the queues is not running yet, so attempting to
	read from the queue set should fail. */
	if( xQueueSelectFromSet( xQueueSet, queuesetSHORT_DELAY ) != NULL )
	{
		xQueueSetTasksStatus = pdFAIL;
	}

	/* Resume the task that writes to the queues. */
	vTaskResume( xQueueSetSendingTask );

	/* Let the ISR access the queues also. */
	xSetupComplete = pdTRUE;
}
Пример #12
0
static void prvLCDTask( void *pvParameters )
{
xQueueMessage xReceivedMessage;

/* Buffer into which strings are formatted and placed ready for display on the
LCD.  Note this is a static variable to prevent it being allocated on the task
stack, which is too small to hold such a variable.  The stack size is configured
when the task is created. */
static char cBuffer[ 512 ];
unsigned char ucLine = 1;


	/* This function is the only function that uses printf().  If printf() is
	used from any other function then some sort of mutual exclusion on stdout
	will be necessary.

	This is also the only function that is permitted to access the LCD.

	First print out the number of bytes that remain in the FreeRTOS heap.  This
	can be viewed in the terminal IO window within the IAR Embedded Workbench. */
	printf( "%d bytes of heap space remain unallocated\n", ( int ) xPortGetFreeHeapSize() );

	/* Just as a test of the port, and for no functional reason, check the task
	parameter contains its expected value. */
	if( pvParameters != mainTASK_PARAMETER_CHECK_VALUE )
	{
		halLcdPrintLine( "Invalid parameter", ucLine,  OVERWRITE_TEXT );
		ucLine++;
	}

	for( ;; )
	{
		/* Wait for a message to be received.  Using portMAX_DELAY as the block
		time will result in an indefinite wait provided INCLUDE_vTaskSuspend is
		set to 1 in FreeRTOSConfig.h, therefore there is no need to check the
		function return value and the function will only return when a value
		has been received. */
		xQueueReceive( xLCDQueue, &xReceivedMessage, portMAX_DELAY );

		/* Clear the LCD if no room remains for any more text output. */
		if( ucLine > mainMAX_LCD_LINES )
		{
			halLcdClearScreen();
			ucLine = 0;
		}

		/* What is this message?  What does it contain? */
		switch( xReceivedMessage.cMessageID )
		{
			case mainMESSAGE_BUTTON_UP		:	/* The button poll task has just
												informed this task that the up
												button on the joystick input has
												been pressed or released. */
												sprintf( cBuffer, "Button up = %d", ( int ) xReceivedMessage.ulMessageValue );
												break;

			case mainMESSAGE_BUTTON_SEL		:	/* The select button interrupt
												just informed this task that the
												select button was pressed.
												Generate a table of task run time
												statistics and output this to
												the terminal IO window in the IAR
												embedded workbench. */
												printf( "\nTask\t     Abs Time\t     %%Time\n*****************************************" );
												vTaskGetRunTimeStats( cBuffer );
												printf( cBuffer );

												/* Also print out a message to
												the LCD - in this case the
												pointer to the string to print
												is sent directly in the
												ulMessageValue member of the
												message.  This just demonstrates
												a different communication
												technique. */
												sprintf( cBuffer, "%s", ( char * ) xReceivedMessage.ulMessageValue );
												break;

			case mainMESSAGE_STATUS			:	/* The tick interrupt hook
												function has just informed this
												task of the system status.
												Generate a string in accordance
												with the status value. */
												prvGenerateStatusMessage( cBuffer, xReceivedMessage.ulMessageValue );
												break;

			default							:	sprintf( cBuffer, "Unknown message" );
												break;
		}

		/* Output the message that was placed into the cBuffer array within the
		switch statement above, then move onto the next line ready for the next
		message to arrive on the queue. */
		halLcdPrintLine( cBuffer, ucLine,  OVERWRITE_TEXT );
		ucLine++;
	}
}
Пример #13
0
/*********************************************************************************************
 * Task to setup traffic light timer to send packets
 *********************************************************************************************/
void prvTrafficLightTask( void *pvParameters ) {

	// Setup timer to start the state machine
	// Send packet every 5 seconds
	sendTrafficLight = swTimerInit( 5000, REPEAT, prvSendTrafficLightCallback );

	// Initialize the first state
	light_system_state = state_1;

	// Initialize the transition times
	timer_NS = 25;
	timer_EW = 30;

	swTimerStart(sendTrafficLight, 0);

	// Let task run infinitely
	for(;;) {

		// Process any incoming packets
		if( process_packet ) {
			// Create local string to represent the packet
			char* packet = pvPortMalloc( MAX_LENGTH*sizeof(uint8_t) );

			// Pop packet from queue
			xQueueReceive( xPacketQueue, packet, 0 );

			// Process packet
			handlePacket( packet );

			// Free variables
			vPortFree( packet );

			// Reset process_packet
			process_packet = FALSE;
		}

		// Send updated traffic packets
		if( update_traffic ) {
			// Light states
			systemState current_lights = current_system_state[light_system_state];

			// Create Header
			Header* traffic_header = pvPortMalloc( sizeof(Header) );
			traffic_header->addr = MY_ADDR;
			traffic_header->mode = allModes;

			// Iterate through all SAVs
			uint8_t sav;
			for(sav=0; sav<NUMBER_SAV; sav++){
				// Check if active
				if( wifi_channel_active[sav] == TRUE ) {
					// Define destination
					traffic_header->dest = sav;

					// Send current state packet
//					traffic_header->type = trafficLightCurrent;
//					sendTrafficLightCurrent( traffic_header, current_lights.northSouth, current_lights.eastWest );

					// Send future state packet
					traffic_header->type = trafficLightFuture;
//					sendTrafficLightFuture( traffic_header, next_light_state[current_lights.northSouth], timer_NS,
//															next_light_state[current_lights.eastWest], timer_EW );
					sendTrafficLightFuture( traffic_header, current_lights.northSouth, timer_NS,
															current_lights.eastWest, timer_EW );
				}
			}

			// Reset flag
			update_traffic = FALSE;

			// Free variables
			vPortFree( traffic_header );
		}
	}
}
Пример #14
0
/**
 * obtain character from the Rx queue
 */
signed portBASE_TYPE xSerialGetChar(xComPortHandle pxPort, signed char *pcRxedChar, TickType_t xBlockTime)
{
    (void)pxPort;
    return xQueueReceive(xRxQueue, pcRxedChar, xBlockTime);
}
Пример #15
0
void vTaskToneGenerator( void *pvParameters )
{
  char dtfmReq;
  portBASE_TYPE xStatus;
  TONE_ON_OFF_TYPE tone_in_progress = TONE_OFF;
  uint8_t activeBuffer  = INVALID_BUFFER;
  uint8_t i = 0;
  t = 0;

  for(i = 0; i < NUM_TONE_BUFFERS; i++)
  {
	  vPrintStringAndNumber("sampleBuf[i] = ", (uint32_t) sampleBuf[i]);
	  bufInUse[i] = 0;
  }

  /* As per most tasks, this task is implemented in an infinite loop. */
  for( ;; )
  {
    /* Check for new input from the queue, don't wait if there is
     * not any new input
     */
    xStatus = xQueueReceive( xQueueToneInput, &dtfmReq, 0 );

    //If something was received from the queue, there is a new command
    //to process
    if( xStatus == pdPASS )
    {
      if(dtfmReq != 0) //Tone On request
      {
        #ifdef DEBUG_TONE_SCHED
          vPrintStringAndNumber("Received new ON request.  Tone =", dtfmReq.tone_type);
        #endif
        //Set tone in progress flag
        tone_in_progress = TONE_ON;

        //Fill buffer with some samples
        FillBuffer(dtfmReq, &activeBuffer);
        #ifdef DEBUG_TONE_SCHED
          vPrintString("   New request.  Samples Added.\n");
        #endif

        SendSamplesToDMA(&activeBuffer);
      }
      else // dtfmReq.on_off == TONE_OFF
      {
        #ifdef DEBUG_TONE_SCHED
          vPrintString("  Tone off\n");
        #endif

          //Clear tone in progress flag
        tone_in_progress = TONE_OFF;
      }
    }
    //Else nothing was received from the queue, continue previous
    //request
    else
    {
      if(tone_in_progress == TONE_ON)
      {
        FillBuffer(dtfmReq, &activeBuffer);
        #ifdef DEBUG_TONE_SCHED
          vPrintString("   No new request.  Samples Added.\n");
        #endif

        SendSamplesToDMA(&activeBuffer);
      }
      //else no active tone ... nothing to play
    }
  }
}
Пример #16
0
void tskHandleRequests(void *pvParameters) {
	packet_t *requestPacket = NULL;

	packet_t *responsePacket = NULL;

	json_t *idObj;
	json_int_t id;


	char *errorString;
	strbuffer_t *errorBuffer;

	strbuffer_t *logMsg;

	portBASE_TYPE xStatus;
	while(1) {
		if(uxQueueMessagesWaiting(requestQueue) > 0) {
			xStatus = xQueueReceive( requestQueue, &requestPacket, (portTickType) QUEUE_RECEIVE_WAIT_TIMEOUT );
			if( (xStatus == pdPASS) && requestPacket) {

				if(check_packet_type(requestPacket, PKG_TYPE_INCOME_JSONRPC_REQUEST) != TRUE) {
					logger(LEVEL_ERR, "Wrong data packet from requestQueue. Deleting packet" );
					packet_destroy(&requestPacket);
					continue;

				}

				logMsg = strbuffer_new();
				strbuffer_append(logMsg, "Received new request: id=");
				strbuffer_append(logMsg, int_to_string(requestPacket->id));
				strbuffer_append(logMsg, " transport=");
				strbuffer_append(logMsg, int_to_string(requestPacket->transport));
				logger(LEVEL_INFO, logMsg->value);
				strbuffer_destroy(&logMsg);


				handle_request(requestPacket);
				responsePacket = requestPacket;
				if(responsePacket) {

					if(requestPacket->type == PKG_TYPE_OUTCOME_JSONRPC_NOTIFICATION) {
						logger(LEVEL_INFO, "Request was notification only. Destroying response");
						packet_destroy(&requestPacket);
						packet_destroy(&responsePacket);
						continue;
					}

					if(check_packet_type(requestPacket, PKG_TYPE_OUTCOME_JSONRPC_RESPONSE) != TRUE) {
						logger(LEVEL_ERR, "Wrong response packet. Deleting packet" );
						packet_destroy(&requestPacket);
						continue;
					}

					logMsg = strbuffer_new();
					strbuffer_append(logMsg, "Received response: id=");
					strbuffer_append(logMsg, int_to_string(responsePacket->id));
					strbuffer_append(logMsg, " transport=");
					strbuffer_append(logMsg, int_to_string(responsePacket->transport));
					logger(LEVEL_INFO, logMsg->value);
					strbuffer_destroy(&logMsg);


					logger(LEVEL_DEBUG, "creating json string from json object");

					strbuffer_t *stringData = strbuffer_new();

					char *jsonData = json_dumps(responsePacket->payload.jsonDoc, JSON_ENCODE_ANY );
					if(jsonData) {
						logger(LEVEL_DEBUG, "json string was created. Creating packet to send.");
						strbuffer_append(stringData, jsonData);

						logMsg = strbuffer_new();
						strbuffer_append(logMsg, "Sending new packet: packet_length=");
						strbuffer_append(logMsg, int_to_string(stringData->length));
						strbuffer_append(logMsg, " packet_transport=");
						strbuffer_append(logMsg, int_to_string(responsePacket->transport));
						logger(LEVEL_DEBUG, logMsg->value);
						strbuffer_destroy(&logMsg);



					} else {
						logMsg = strbuffer_new();
						strbuffer_append(logMsg, "Unable to create string from response json: id=");
						strbuffer_append(logMsg, int_to_string(responsePacket->id));
						strbuffer_append(logMsg, " transport=");
						strbuffer_append(logMsg, int_to_string(responsePacket->transport));
						logger(LEVEL_WARN, logMsg->value);
						strbuffer_destroy(&logMsg);

						errorString = format_jsonrpc_error(JSONRPC_SERVER_ERROR, MSG_JSONRPC_ERRORS.server_error, "internal server error", responsePacket->id);
						strbuffer_append(stringData, errorString);
					}

					vPortFree(jsonData);

					packet_lock(responsePacket);
					{
						json_decref(responsePacket->payload.jsonDoc);

						responsePacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
						responsePacket->payload.stringData = stringData;
					}
					packet_unlock(responsePacket);


					sendOutputMessage(responsePacket);

//					idObj = json_object_get(responseJson, "id");
//					id = json_integer_value(idObj);
//
//					json_t *transportObject = json_object_get(responseJson, "transport");
// 					json_int_t transport = json_integer_value(transportObject);
//
//					logger_format(LEVEL_INFO, "Received response. id = %d transport=%s", (int) id, transport_type_to_str((transport_type_t) transport));
//
//					xStatus = xQueueSendToBack( responseQueue, &responseJson, (portTickType) QUEUE_SEND_WAIT_TIMEOUT );
//					if( xStatus != pdPASS ){
//						char* idStr = json_dumps(json_object_get(requestPacket, "id"), JSON_ENCODE_ANY);
//						json_int_t transport = json_integer_value(json_object_get(requestPacket, "transport"));
//						json_object_del(requestPacket, "transport");
//						json_decref(requestJson);
//						// send using error reporting function
//						if(id && transport) {
//							report_error_to_sender(
//								(transport_type_t) transport,
//								MSG_JSONRPC_ERRORS.general_error_json,
//								JSONRPC_SERVER_ERROR,				/* <-- code */
//								MSG_JSONRPC_ERRORS.server_error,	/* <-- message */
//								MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout , /* <-- data */
//								idStr
//							);
//						}
//						if(idStr) vPortFree(idStr);
//						logger_format(LEVEL_INFO, "Unable to add response to queue. Request id = %d", (int) id );
//					} else {
//						logger(LEVEL_DEBUG, "Response was added to response queue");
//					}

				}
//				json_decref(requestPacket);
			}
		}
		taskYIELD();
	}

}
Пример #17
0
/*-----------------------------------------------------------*/
void i2cMessage( const unsigned char * const pucMessage, long lMessageLength, unsigned char ucSlaveAddress, unsigned short usBufferAddress, unsigned long ulDirection, xSemaphoreHandle xMessageCompleteSemaphore, portTickType xBlockTime )
{
extern volatile xI2CMessage *pxCurrentMessage;
xI2CMessage *pxNextFreeMessage;
signed portBASE_TYPE xReturn;

	portENTER_CRITICAL();
	{
		/* This message is guaranteed to be free as there are two more messages
		than spaces in the queue allowing for one message to be in process of
		being transmitted and one to be left free. */
		pxNextFreeMessage = &( xTxMessages[ ulNextFreeMessage ] );

		/* Fill the message with the data to be sent. */

		/* Pointer to the actual data.  Only a pointer is stored (i.e. the 
		actual data is not copied, so the data being pointed to must still
		be valid when the message eventually gets sent (it may be queued for
		a while. */
		pxNextFreeMessage->pucBuffer = ( unsigned char * ) pucMessage;		

		/* This is the address of the I2C device we are going to transmit this
		message to. */
		pxNextFreeMessage->ucSlaveAddress = ucSlaveAddress | ( unsigned char ) ulDirection;

		/* A semaphore can be used to allow the I2C ISR to indicate that the
		message has been sent.  This can be NULL if you don't want to wait for
		the message transmission to complete. */
		pxNextFreeMessage->xMessageCompleteSemaphore = xMessageCompleteSemaphore;

		/* How many bytes are to be sent? */
		pxNextFreeMessage->lMessageLength = lMessageLength;

		/* The address within the WIZnet device to which the data will be 
		written.  This could be the address of a register, or alternatively
		a location within the WIZnet Tx buffer. */
		pxNextFreeMessage->ucBufferAddressLowByte = ( unsigned char ) ( usBufferAddress & 0xff );

		/* Second byte of the address. */
		usBufferAddress >>= 8;
		pxNextFreeMessage->ucBufferAddressHighByte = ( unsigned char ) ( usBufferAddress & 0xff );

		/* Increment to the next message in the array - with a wrap around check. */
		ulNextFreeMessage++;
		if( ulNextFreeMessage >= ( i2cQUEUE_LENGTH + i2cEXTRA_MESSAGES ) )
		{
			ulNextFreeMessage = ( unsigned long ) 0;
		}

		/* Is the I2C interrupt in the middle of transmitting a message? */
		if( *pulBusFree == ( unsigned long ) pdTRUE )
		{
			/* No message is currently being sent or queued to be sent.  We
			can start the ISR sending this message immediately. */
			pxCurrentMessage = pxNextFreeMessage;

			I2C_I2CONCLR = i2cSI_BIT;	
			I2C_I2CONSET = i2cSTA_BIT;
			
			*pulBusFree = ( unsigned long ) pdFALSE;
		}
		else
		{
			/* The I2C interrupt routine is mid sending a message.  Queue
			this message ready to be sent. */
			xReturn = xQueueSend( xMessagesForTx, &pxNextFreeMessage, xBlockTime );

			/* We may have blocked while trying to queue the message.  If this
			was the case then the interrupt would have been enabled and we may
			now find that the I2C interrupt routine is no longer sending a
			message. */
			if( ( *pulBusFree == ( unsigned long ) pdTRUE ) && ( xReturn == pdPASS ) )
			{
				/* Get the next message in the queue (this should be the 
				message we just posted) and start off the transmission
				again. */
				xQueueReceive( xMessagesForTx, &pxNextFreeMessage, i2cNO_BLOCK );
				pxCurrentMessage = pxNextFreeMessage;

				I2C_I2CONCLR = i2cSI_BIT;	
				I2C_I2CONSET = i2cSTA_BIT;
				
				*pulBusFree = ( unsigned long ) pdFALSE;
			}
		}
	}
	portEXIT_CRITICAL();
}
Пример #18
0
void tskParseJson(void *pvParameters) {


	packet_t *incomePacket = NULL;
	json_t 	*requestJson = NULL;
	const char *transportString;

	char *errorString;
	strbuffer_t *errorBuffer;

	strbuffer_t *logMsg;

	portBASE_TYPE xStatus;
	while(1) {
		if(uxQueueMessagesWaiting(msgIncomeQueue) > 0) {
			xStatus = xQueueReceive( msgIncomeQueue, &incomePacket, (portTickType) QUEUE_RECEIVE_WAIT_TIMEOUT );
			if( (xStatus == pdPASS) && incomePacket) {

				if(incomePacket->type != PKG_TYPE_INCOME_MESSAGE_STRING) {
					logMsg = strbuffer_new();
					strbuffer_append(logMsg, "Received packet with wrong type. Got ");
					strbuffer_append(logMsg, int_to_string(incomePacket->type));
					strbuffer_append(logMsg, " instead of ");
					strbuffer_append(logMsg, int_to_string(PKG_TYPE_INCOME_MESSAGE_STRING));
					logger(LEVEL_ERR, logMsg->value);
					strbuffer_destroy(&logMsg);


					errorString = format_jsonrpc_error(JSONRPC_SERVER_ERROR, MSG_JSONRPC_ERRORS.server_error, "internal server error", 0);
					errorBuffer = strbuffer_new();
					strbuffer_append(errorBuffer, errorString);

					packet_t * errorPacket = packet_new();

					errorPacket->type = PKG_TYPE_OUTGOING_MESSAGE_STRING;
					errorPacket->payload.stringData = errorBuffer;
					errorPacket->transport = incomePacket->transport;

					sendOutputMessage(errorPacket);

					packet_destroy(&incomePacket);

					continue;
				}

				logMsg = strbuffer_new();
				strbuffer_append(logMsg, "Received packet. len = ");
				strbuffer_append(logMsg, int_to_string(incomePacket->payload.stringData->length));
				strbuffer_append(logMsg, "   transport=");
				strbuffer_append(logMsg, int_to_string(incomePacket->transport));
				logger(LEVEL_INFO, logMsg->value);
				strbuffer_destroy(&logMsg);

				parseJsonPacket(incomePacket);

				strbuffer_t *logMsg;
				switch(incomePacket->type) {
				case PKG_TYPE_OUTGOING_MESSAGE_STRING:
					logger(LEVEL_WARN, "Unable to parse incoming json packet. Sending message to client");
					sendOutputMessage(incomePacket);
					break;

				case PKG_TYPE_INCOME_JSONRPC_REQUEST:
					logMsg = strbuffer_new();
					strbuffer_append(logMsg, "New jsonrpc request. id=");
					strbuffer_append(logMsg, int_to_string(incomePacket->id));
					strbuffer_append(logMsg, "transport=");
					strbuffer_append(logMsg, int_to_string(incomePacket->transport));

					logger(LEVEL_INFO, logMsg->value);
					strbuffer_destroy(&logMsg);
					//logger_format(LEVEL_INFO, "New jsonrpc request. id=%d transport=%s", incomePacket->id, transportString);
					add_new_rpc_request(incomePacket);
					break;

				default:
					logMsg = strbuffer_new();
					strbuffer_append(logMsg, "Wrong packet type was returned fron json parse function. type_returned=");
					strbuffer_append(logMsg, int_to_string(incomePacket->type));
					logger(LEVEL_ERR, logMsg->value);
					strbuffer_destroy(&logMsg);
					packet_destroy(&incomePacket);
					break;
				}
//				if(requestJson) {
//					logger_format(LEVEL_INFO, "Parsing packet was successful.");
//
////					json_incref(requestJson);
//					logger(LEVEL_DEBUG, "Adding json to request queue");
//					xStatus = xQueueSendToBack( requestQueue, &requestJson, (portTickType) QUEUE_SEND_WAIT_TIMEOUT );
//					if( xStatus != pdPASS ){
//
//						json_object_del(requestJson, "method");
//						json_object_del(requestJson, "params");
//
//						json_t* errorObj = create_error(JSONRPC_SERVER_ERROR, MSG_JSONRPC_ERRORS.server_error);
//						json_object_set_new(errorObj, "data", json_string(MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout));
//
//						json_object_set_new(requestJson, "error", errorObj);
//
//						logger_format(LEVEL_WARN, "%s", MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout);
//
//						// send error back to client using output queue
//						xStatus = xQueueSendToBack( responseQueue, &requestJson, (portTickType) QUEUE_SEND_WAIT_TIMEOUT );
//						if( xStatus != pdPASS ){
//							char* id = json_dumps(json_object_get(requestJson, "id"), JSON_ENCODE_ANY);
//							transport_type_t transport = json_integer_value(json_object_get(requestJson, "transport"));
//							json_object_del(requestJson, "transport");
//							json_decref(requestJson);
//							// send using error reporting function
//							if(id && transport) {
//								report_error_to_sender(
//									transport,
//									MSG_JSONRPC_ERRORS.general_error_json,
//									JSONRPC_SERVER_ERROR,				/* <-- code */
//									MSG_JSONRPC_ERRORS.server_error,	/* <-- message */
//									MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout , /* <-- data */
//									id
//								);
//							}
//							if(id) vPortFree(id);
//							logger_format(LEVEL_WARN, "%s", MSG_MAINTASKS.tskAbstractReader.device_is_busy_timeout);
//						}
//					} else {
//						logger(LEVEL_DEBUG, "Json was added to request queue");
//					}
//				}
//				json_decref(requestJson);
			}
		}
		taskYIELD();
	}
}
Пример #19
0
/*
 * fill INS_delay_buffer
 * wait GPS signal
 */
void vINSAligTask(void* pvParameters)
{
	char printf_buffer[100];
	
	/*odometry sensor data*/
	float direction;
	u8 temp;
	float *p_insBuffer;
	GPSDataType gdt;
	u16 GPS_validate_cnt=0;
	float uw_height;
	
	portBASE_TYPE xstatus;
	
	/*Enable ultrasonic sensor TIMER*/
	TIM2_Config();
	TIM2_IT_Config();
	
	/**/
	xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);	//capture an INS frame	 
	p_insBuffer[INDEX_DT]=0.0;	//the last number in buffer represent time interval, not time
	Blinks(LED1,2);

#ifdef INS_DEBUG
	/*GPS data is not needed in debug mode*/
	while(1)
	{		
		/*receive ins data and fill the IMU_delay_buffer*/
		xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
		PutToBuffer(p_insBuffer);
		/*clear time interval*/
		p_insBuffer[INDEX_DT]=0.0;
		/*INS_delay_buffer is full filled*/
		if(buffer_header == 0) break;
	}
	
	navParamK[0] = 0.0;
	navParamK[1] = 0.0;
	navParamK[2] = 0.0;
	navParamK[3] = 0.0;
	navParamK[4] = 0.0;
	navParamK[5] = 0.0;
	navParamK[6] = 0.0;
	navParamK[7] = 0.0;
	navParamK[8] = 0.0;
	
	x[0]=0.0;
	x[1]=0.0;
	x[2]=0.0;
	x[3]=0.0;
	x[4]=0.0;
	x[5]=0.0;
	x[6]=0.0;
	x[7]=0.0;
	x[8]=0.0;
#else
	//normol mode
	
	/*wait while GPS signal not stable*/
	while(GPS_validate_cnt<=100)
	{
		xstatus = xQueueReceive(xUartGPSQueue,&gdt,0);
		if(xstatus == pdPASS)
		{
			GPS_validate_cnt ++;
		}
		if(GetUltraSonicMeasure(&uw_height))
		{
			sprintf(printf_buffer,"%1.3f\r\n",uw_height);
			UartSend(printf_buffer,7);
		}
		
		/*receive ins data and fill the IMU_delay_buffer*/
		xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY);
		PutToBuffer(p_insBuffer);
		/*clear time interval*/
		p_insBuffer[INDEX_DT]=0.0;
	}

	/************initialize navParamK*********************/
	temp=(u8)(gdt.Lati*0.01);
	initPos[0]=0.01745329*(temp+(gdt.Lati-temp*100.0)*0.0166667);

	temp=(u8)(gdt.Long*0.01);
	initPos[1]=0.01745329*(temp+(gdt.Long-temp*100.0)*0.0166667);
	initPos[2]=gdt.Alti;

	if(gdt.type != GPGMV)
	{
		direction = gdt.COG*0.0174533;
		gdt.speedN = gdt.SPD*0.51444*arm_cos_f32(direction);
		gdt.speedE = gdt.SPD*0.51444*arm_sin_f32(direction);
	}
	
	navParamK[0] = 0.0;
	navParamK[1] = 0.0;
	navParamK[2] = 0.0;
	navParamK[3] = gdt.speedN;
	navParamK[4] = gdt.speedE;
	navParamK[5] = 0.0;
	navParamK[6] = 0.0;
	navParamK[7] = 0.0;
	navParamK[8] = 0.0;
	
	/*initialize filter state param x*/
	x[0]=0.0;
	x[1]=0.0;
	x[2]=0.0;
	x[3]=0.0;
	x[4]=0.0;
	x[5]=0.0;
	x[6]=0.0;
	x[7]=0.0;
	x[8]=0.0;
#endif	
	xstatus=xTaskCreate(vIEKFProcessTask,(signed portCHAR *)"ins_ekf",configMINIMAL_STACK_SIZE+1024,(void *)NULL,tskIDLE_PRIORITY+1,NULL);
	if(xstatus!=pdTRUE)
	{
		sprintf(printf_buffer, "failed to initialize\r\n");
		UartSend(printf_buffer, strlen(printf_buffer));
	}
	vTaskDelete(NULL);
}
Пример #20
0
static void prvHighestPriorityPeekTask( void *pvParameters )
{
xQueueHandle xQueue = ( xQueueHandle ) pvParameters;
unsigned portLONG ulValue;

	#ifdef USE_STDIO
	{
		void vPrintDisplayMessage( const portCHAR * const * ppcMessageToSend );
	
		const portCHAR * const pcTaskStartMsg = "Queue peek test started.\r\n";

		/* Queue a message for printing to say the task has started. */
		vPrintDisplayMessage( &pcTaskStartMsg );
	}
	#endif

	for( ;; )
	{
		/* Try peeking from the queue.  The queue should be empty so we will
		block, allowing the high priority task to execute. */
		if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
		{
			/* We expected to have received something by the time we unblock. */
			xErrorDetected = pdTRUE;
		}

		/* When we reach here the high and medium priority tasks should still
		be blocked on the queue.  We unblocked because the low priority task
		wrote a value to the queue, which we should have peeked.  Peeking the
		data (rather than receiving it) will leave the data on the queue, so
		the high priority task should then have also been unblocked, but not
		yet executed. */
		if( ulValue != 0x11223344 )
		{
			/* We did not receive the expected value. */
			xErrorDetected = pdTRUE;
		}

		if( uxQueueMessagesWaiting( xQueue ) != 1 )
		{
			/* The message should have been left on the queue. */
			xErrorDetected = pdTRUE;
		}

		/* Now we are going to actually receive the data, so when the high
		priority task runs it will find the queue empty and return to the
		blocked state. */
		ulValue = 0;
		if( xQueueReceive( xQueue, &ulValue, qpeekNO_BLOCK ) != pdPASS )
		{
			/* We expected to receive the value. */
			xErrorDetected = pdTRUE;
		}

		if( ulValue != 0x11223344 )
		{
			/* We did not receive the expected value - which should have been
			the same value as was peeked. */
			xErrorDetected = pdTRUE;
		}

		/* Now we will block again as the queue is once more empty.  The low 
		priority task can then execute again. */
		if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
		{
			/* We expected to have received something by the time we unblock. */
			xErrorDetected = pdTRUE;
		}

		/* When we get here the low priority task should have again written to the
		queue. */
		if( ulValue != 0x01234567 )
		{
			/* We did not receive the expected value. */
			xErrorDetected = pdTRUE;
		}

		if( uxQueueMessagesWaiting( xQueue ) != 1 )
		{
			/* The message should have been left on the queue. */
			xErrorDetected = pdTRUE;
		}

		/* We only peeked the data, so suspending ourselves now should enable
		the high priority task to also peek the data.  The high priority task
		will have been unblocked when we peeked the data as we left the data
		in the queue. */
		vTaskSuspend( NULL );



		/* This time we are going to do the same as the above test, but the
		high priority task is going to receive the data, rather than peek it.
		This means that the medium priority task should never peek the value. */
		if( xQueuePeek( xQueue, &ulValue, portMAX_DELAY ) != pdPASS )
		{
			xErrorDetected = pdTRUE;
		}

		if( ulValue != 0xaabbaabb )
		{
			xErrorDetected = pdTRUE;
		}

		vTaskSuspend( NULL );		
	}
}