コード例 #1
0
ファイル: main.c プロジェクト: pbouffard/asctec_mav_framework
void mainloop(void)
{
  static unsigned char led_cnt = 0, led_state = 1;

  led_cnt++;

  if ((GPS_Data.status & 0xFF) == 0x03)
  {
    LED(0, OFF);
  }
  else
  {
    if (led_cnt == 150)
    {
      LED(0, ON);
    }
    else if (led_cnt == 200)
    {
      led_cnt = 0;
      LED(0, OFF);
    }
  }

  SDK_mainloop();

  HL2LL_write_cycle(); //write data to transmit buffer for immediate transfer to LL processor

  if (GPS_init_status == GPS_NEEDS_CONFIGURATION) //configuration SM of GPS at startup
  {
    GPS_configure();
  }

  if (gpsDataOkTrigger)
  {
    if (GPS_Data.horizontal_accuracy > 12000)
      GPS_Data.status &= ~0x03;
    if (GPS_timeout > 50)//(GPS_Data.status&0xFF)!=0x03)
    {
      if (led_state)
      {
        led_state = 0;
        LED(1, OFF);
      }
      else
      {
        LED(1, ON);
        led_state = 1;
      }
    }
    GPS_timeout = 0;
    if (GPS_init_status == GPS_STARTUP)
      GPS_init_status = GPS_IS_CONFIGURED; //gps config valid, if received correct packet
    HL_Status.latitude = GPS_Data.latitude;
    HL_Status.longitude = GPS_Data.longitude;

  }

}
コード例 #2
0
ファイル: main.c プロジェクト: xun-tong/AscTec_SDK_v3.0
void mainloop(void) //mainloop is triggered at 1 kHz
{
    static unsigned char led_cnt=0, led_state=1;
    static int Firefly_led_fin_cnt=0;
	unsigned char t;

	//blink red led if no GPS lock available
	led_cnt++;
	if((GPS_Data.status&0xFF)==0x03)
	{
		LED(0,OFF);
	}
	else
	{
	    if(led_cnt==150)
	    {
	      LED(0,ON);
	    }
	    else if(led_cnt==200)
	    {
	      led_cnt=0;
	      LED(0,OFF);
	    }
	}


	//after first lock, determine magnetic inclination and declination
	if (SYSTEM_initialized)
	{
		if ((!declinationAvailable) && (GPS_Data.horizontal_accuracy<10000) && ((GPS_Data.status&0x03)==0x03)) //make sure GPS lock is valid
		{
			int status;
			estimatedDeclination=getDeclination(GPS_Data.latitude,GPS_Data.longitude,GPS_Data.height/1000,2012,&status);
			if (estimatedDeclination<-32000) estimatedDeclination=-32000;
			if (estimatedDeclination>32000) estimatedDeclination=32000;
			declinationAvailable=1;
		}
	}

	//toggle green LED and update SDK input struct when GPS data packet is received
    if (gpsLEDTrigger)
    {
		if(led_state)
		{
			led_state=0;
			LED(1,OFF);
		}
		else
		{
			LED(1,ON);
			led_state=1;
		}

		RO_ALL_Data.GPS_height=GPS_Data.height;
		RO_ALL_Data.GPS_latitude=GPS_Data.latitude;
		RO_ALL_Data.GPS_longitude=GPS_Data.longitude;
		RO_ALL_Data.GPS_speed_x=GPS_Data.speed_x;
		RO_ALL_Data.GPS_speed_y=GPS_Data.speed_y;
		RO_ALL_Data.GPS_status=GPS_Data.status;
		RO_ALL_Data.GPS_sat_num=GPS_Data.numSV;
		RO_ALL_Data.GPS_week=GPS_Time.week;
		RO_ALL_Data.GPS_time_of_week=GPS_Time.time_of_week;
		RO_ALL_Data.GPS_heading=GPS_Data.heading;
		RO_ALL_Data.GPS_position_accuracy=GPS_Data.horizontal_accuracy;
		RO_ALL_Data.GPS_speed_accuracy=GPS_Data.speed_accuracy;
		RO_ALL_Data.GPS_height_accuracy=GPS_Data.vertical_accuracy;

		gpsLEDTrigger=0;
    }

	//re-trigger UART-transmission if it was paused by modem CTS pin
	if(trigger_transmission)
	{
		if(!(IOPIN0&(1<<CTS_RADIO)))
	  	{
	  		trigger_transmission=0;
		    if(ringbuffer(RBREAD, &t, 1))
		    {
		      transmission_running=1;
		      UARTWriteChar(t);
		    }
	  	}
	}

#ifdef MATLAB
	//re-trigger UART-transmission if it was paused by modem CTS pin
	if(trigger_transmission)
	{
		if(!(IOPIN0&(1<<CTS_RADIO)))
	  	{
	  		trigger_transmission=0;
		    if(UART_Matlab_fifo(RBREAD, &t, 1))
		    {
		      transmission_running=1;
		      UARTWriteChar(t);
		    }
	  	}
	}
#endif

	//send data packet as an example how to use HL_serial_0 (please refer to uart.c for details)
/*
    if(uart_cnt++==ControllerCyclesPerSecond/DataOutputsPerSecond)
    {
    	uart_cnt=0;
      	if((sizeof(RO_ALL_Data))<ringbuffer(RBFREE, 0, 0))
       	{
       		UART_SendPacket(&RO_ALL_Data, sizeof(RO_ALL_Data), PD_RO_ALL_DATA);
       	}
    }
*/
    //handle gps data reception
    uBloxReceiveEngine();

	//run SDK mainloop. Please put all your data handling / controller code in sdk.c
	SDK_mainloop();

    //write data to transmit buffer for immediate transfer to LL processor
    HL2LL_write_cycle();

    //control pan-tilt-unit ("cam option 4" @ AscTec Pelican and AscTec Firefly)
    PTU_update();

    //synchronize all variables, commands and parameters with ACI
    aciSyncVar();
    aciSyncCmd();
    aciSyncPar();

    //run ACI engine
    aciEngine();

    //send buildinfo
    if ((SYSTEM_initialized) && (!transmitBuildInfoTrigger))
		transmitBuildInfoTrigger=1;

    //Firefly LED
    if (SYSTEM_initialized&&fireflyLedEnabled)
    {
    	if(++Firefly_led_fin_cnt==10)
    	{
    		Firefly_led_fin_cnt=0;
    		fireFlyLedHandler();
    	}
    }

}