Ejemplo n.º 1
0
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 );
	}
}
Ejemplo n.º 2
0
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;
		}
	}
}
Ejemplo n.º 3
0
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;

}
Ejemplo n.º 4
0
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;
		}
	}
}
Ejemplo n.º 5
0
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);
   }
}
Ejemplo n.º 7
0
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 );
	}
}
Ejemplo n.º 8
0
/**
 *
 * @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);
}
Ejemplo n.º 9
0
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);
		}
	}
}
Ejemplo n.º 10
0
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);
    }
}
Ejemplo n.º 11
0
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
}
Ejemplo n.º 12
0
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() );
	}
}
Ejemplo n.º 13
0
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);
			}
		}
	}
}
Ejemplo n.º 15
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 );
	}
}
Ejemplo n.º 16
0
/**
 * 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
}
Ejemplo n.º 17
0
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 );
		}
	}
}
Ejemplo n.º 18
0
/////////////////////////////////////////////////////////////////////////////
// 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;
  }
}
Ejemplo n.º 19
0
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 ..
        }
    }
}
Ejemplo n.º 20
0
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
}
Ejemplo n.º 21
0
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;
    }
}
Ejemplo n.º 22
0
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);
	}
}
Ejemplo n.º 23
0
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;
		}
	}
}
Ejemplo n.º 24
0
Archivo: main.c Proyecto: Eclo/FreeRTOS
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: " );
			LCD_Character_Display_PrintNumber( ulIteration++ );
			LCD_Character_Display_Position( ( ++ulRow ) & 0x1, 0 );
			LCD_Character_Display_PrintString( "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 );
		}
	}
}
Ejemplo n.º 26
0
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();
      }
    }
  }
}
Ejemplo n.º 27
0
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 ) );


    }

}
Ejemplo n.º 28
0
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 );
	}
}
Ejemplo n.º 29
0
//*****************************************************************************
//
// 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);
    }
}
Ejemplo n.º 30
0
/**
 * 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);
	}
}