/**
  * @brief  Main program.
  * @param  None
  * @retval None
  */
uint32_t main(void)
{
  RST_CLK_DeInit();
  /* Select HSI/2 as CPU_CLK source*/
  RST_CLK_CPU_PLLconfig (RST_CLK_CPU_PLLsrcHSIdiv2,0);
  /* Periph clocks enable */
  RST_CLK_PCLKcmd((RST_CLK_PCLK_RST_CLK | RST_CLK_PCLK_CAN1),ENABLE);
  RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTD,ENABLE);

  /* Reset PORTD settings */
  PORT_DeInit(MDR_PORTD);

  /* Configure PORTD pins 10,11 for output to switch LED on/off */
  PORT_InitStructure.PORT_FUNC  = PORT_FUNC_PORT;
  PORT_InitStructure.PORT_Pin   = PORT_Pin_10 | PORT_Pin_11;
  PORT_InitStructure.PORT_OE    = PORT_OE_OUT;
  PORT_InitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
  PORT_InitStructure.PORT_SPEED = PORT_SPEED_SLOW;
  PORT_Init(MDR_PORTD, &PORT_InitStructure);

/* CAN transmit at 125Kb/s and receive by polling in loopback mode */
  TestRx = CAN_Polling();
  if (TestRx == FAILED)
  {
    /* Turn on led LED2 */
    LEDOn(LED2);
  }
  else
  {
    /* Turn on led LED1 */
    LEDOn(LED1);
  }

  while(1);
}
Beispiel #2
0
void task0( void * pdata )
{
    while(1)
    {
        //printf("task0\r\n");
#if ONE_FRAME_EN
        //UART_Print("task0\r\n");
        //UART_Print("AT\r\n");
#endif
        //__disable_interrupt();
        //LED0_On();
        LEDOn(RED);
        //__enable_interrupt();
        OSTimeDlyHMSM(0,0,1,920);
        //OSTimeDlyHMSM(0,0,0,TIME_DELAY);

#if 1
        //__disable_interrupt();
        //LED0_Off();
        LEDOn(GREEN);
        OSTimeDlyHMSM(0,0,1,920);
        //__enable_interrupt();

        //OSTimeDlyHMSM(0,0,1,TIME_DELAY );
#endif
    }

}
/**
  * @brief  Indicate error condition with LED3
  * @param  None
  * @retval None
  */
void IndicateError(void)
{
  /* Switch LED3 on and off in case of error */
  LEDOn(LED3);
  Delay(BLINK_DELAY);
  LEDOff(LED3);
}
Beispiel #4
0
/**
	@brief Sends a photo via telemetry
	@warning Executing this function takes ages. (About 0,5 to 4 seconds, depending on the resolution.)
*/
void sendPhoto(uint16_t image_size_send, uint8_t ** current_image, uint8_t ** previous_image){
	uint16_t image_width_send;
	uint16_t image_height_send;

	image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
	image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
	//delay(250);
	mavlink_msg_data_transmission_handshake_send(
			MAVLINK_COMM_2,
			MAVLINK_DATA_STREAM_IMG_RAW8U,
			image_size_send,
			image_width_send,
			image_height_send,
			image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
			MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
			100);
	//send_image_now = false;
	
	uint16_t frame = 0;
	//delay(250);
	
	dma_copy_image_buffers(current_image, previous_image, image_size_send, 1);

	for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
	{
		LEDOn(LED_ERR);
		mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, ((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
		LEDOff(LED_ERR);
		//delay(250);
		//send_image_now = false;
	}
}
Beispiel #5
0
void MinuteView(void)
  {
       unsigned int OnTime = 10*minute;
       unsigned int OffTime = 120/minute;
         for(LEDPointer=1, bitpointer=1;LEDPointer <= 6;LEDPointer++)
         {
           if(minute & bitpointer)
           {
             LEDOn();
           }
             delay(OnTime);
           if(!(minute & bitpointer))
           {
             LEDOff();
           }
             delay(OffTime);

            // All LEDs Off
             P1DIR &= (~LED_MASK);
             P1OUT &= (~LED_MASK);

           bitpointer<<=1;
           if (bitpointer==64) bitpointer=1;
         }
  }
Beispiel #6
0
void mpu6050_enable()
{
	uint8_t data = 0;	
	bool res;
	res = I2C_WriteBytes(I2C2, &data, MPU_ADDR,0x6B, 1);
	if (!res)
	{
		LEDOn(LED_ERR);		
	}	
}
/**
  * @brief  Blink with LED1
  * @param  num - blinks number
  * @param  del - delay
  * @retval None
  */
void BlinkLED1(uint32_t num, uint32_t del)
{
uint32_t cnt;

  for ( cnt = 0; cnt < num; cnt++)
  {
    LEDOn(LED1);
    Delay(del);
    LEDOff(LED1);
    Delay(del);
  }
}
Beispiel #8
0
void ShowStatus() {
    uint8_t i;
    if(LEDStatus == 0 || lastWhistleCount != whistleCount) {
        statusChange += 1;
        AllLEDOff();
        for (i = 0; i < whistleCount; i++) {
            LEDOn(i);
        }
    }
    LEDStatus = 1;
    lastWhistleCount = whistleCount;
}
Beispiel #9
0
int main()
{

    init();
    printf("System On and Finish Init\r\n");
    LEDOn(LED1);
    LEDOn(LED2);


    _file_system_init();

    printf("Init done\r\n");
    SonarInit();
    RFIDSendCommand(kRFIDOn);
    RFIDSetCallback(_cbRFID);
    RFIDSetCallback(_cbSonar);

    /*LEDDecoderSet(1);*/

    /*while (1) {*/
        
        /*printf("Dis = %d\r\n", SonarRead());*/
        
        /*sleep(100);*/
    /*}*/


    /*int res = GUI_GIF_Draw((const void*) bmpfile, rd, 50, 50);*/

    /* Tasks */
    xTaskCreate(heartbeat, "heartbeat", 200, NULL, 1, NULL);
    xTaskCreate(readUART1, "UART1", 300, NULL, 1, NULL);
    xTaskCreate(readUART3, "UART3", 300, NULL, 1, NULL);
    /*xTaskCreate(SonarScanTask, "sonar", 200, NULL, 1, NULL);*/
    xTaskCreate(GUITask, "gui_main", 1000, NULL, 1, NULL);
    xTaskCreate(touchScreenUpdate, "touch", 200, NULL, 1, NULL);
    
    vTaskStartScheduler();  
}
void main(void)
#endif
{
  /* Enables the HSI clock for PORTB */
  RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTB, ENABLE);

  /* Configure all unused PORT pins to low power consumption */

  PORT_StructInit(&PORT_InitStructure);
  PORT_InitStructure.PORT_Pin = (PORT_Pin_All & ~(PORT_Pin_0));
  PORT_Init(MDR_PORTB, &PORT_InitStructure);

  /* Configure PORTB pin 10 for output to switch LEDs on/off */

  PORT_InitStructure.PORT_Pin   = PORT_Pin_0;
  PORT_InitStructure.PORT_OE    = PORT_OE_OUT;
  PORT_InitStructure.PORT_FUNC  = PORT_FUNC_PORT;
  PORT_InitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
  PORT_InitStructure.PORT_SPEED = PORT_SPEED_SLOW;

  PORT_Init(MDR_PORTB, &PORT_InitStructure);

  /* Enables the HSI clock for WWDG */
  RST_CLK_PCLKcmd(RST_CLK_PCLK_WWDG,ENABLE);

  NVIC_EnableIRQ(WWDG_IRQn);

  /* Set WWDG Prescaler value*/
  WWDG_SetPrescaler	(WWDG_Prescaler_8);

  /* Enable WWDG and load start counter value*/
  WWDG_Enable(0x7F);

  /* Enables the WWDG Early Wakeup interrupt */
  WWDG_EnableIT();

  /* Infinite loop */
  for(NumBlink =  0; NumBlink < 15;)
  {
    if (wwdg_flag == SET)
    {
      BlinkLED1(1,30000);
      wwdg_flag = RESET;
    }
  }
  LEDOn(LED1);
  NVIC_DisableIRQ(WWDG_IRQn);
  while(1);
}
Beispiel #11
0
void HourView(void)
  {
    j = 5000/hour;
     for(i=0;i<j;i++)
     {
       unsigned int OnTime = 10*hour;
       unsigned int OffTime = 120/hour;
         for(LEDPointer = 1;LEDPointer <= hour;LEDPointer++)
         {
             LEDOn();
             delay(OnTime);
             LEDOff();
             delay(OffTime);
         }
     }
  }
/**
 * @fn void Alarm(unsigned int * pBuffer)
 *
 * @brief Alarm implementation
 *
 * @param pBuffer an unsigned integer pointer
 */
void Alarm(unsigned int * pBuffer)
{
  unsigned int r = 0;
  unsigned char i;
  for (i=1;i<MAX_NODES;i++) {
    if (Nodes[i].ID) {                                                          // Is it a valid network member
      r |= Nodes[i].Data[2] & 0x8000;                                           // 'OR' all red LED's (alarm)
    }
  }
  Nodes[0].Data[2] = (Nodes[0].Data[2] & (~0x8000)) | r;                        // Update our local copy for a red LED Alarm
  if (r) {                                                                      // Write to the HW the red LED (redundant)
    LEDOn(__BSP_LEDRED1);
  } else {
    LEDOff(__BSP_LEDRED1);
  }
}
Beispiel #13
0
void JPEG_encode(void)//编码主函数
{

	u8 res;
	u16 i,j,color;

	res=f_mount(0, &fatfs);
	while(res)LCD_String(20,20,"SD mount failed!",RED);
	res=f_open(&fileW,"0:/DCMI/ph1.jpg",FA_WRITE|FA_CREATE_ALWAYS);//创建并打开
	while(res)LCD_String(20,20,"file create failed!",RED);

	cinfo=jpeg_create_compress();//创建JPEG压缩文件
	cinfo->image_width=240;
  	cinfo->image_height=320;
  	cinfo->output=0;//数据输出到NULL;
  	jpeg_set_default(cinfo);//设置默认参数
	jpeg_start_compress(cinfo);//开始压缩,写压缩文件头信息
	j=0;
	while(cinfo->next_line<cinfo->image_height)
	{
		for(i=0;i<240;i++)//读取一行
		{
			LCD_Cursor(i,cinfo->next_line);			
			if((lcdid&0xff00)==0x9300)color=BGR565toRGB565(LCD_ReadRAM());
			else color=LCD_ReadRAM();
			cinfo->inbuf[j++]=(u8)((color&0xf800)>>8);
			cinfo->inbuf[j++]=(u8)((color&0x07e0)>>3);
			cinfo->inbuf[j++]=(u8)((color&0x001f)<<3);
		}
		cinfo->next_line++; 
	  	//当数据填满时压缩并输出数据(填满16行)
	  	if(cinfo->next_line%cinfo->inbuf_height==0)
		{
			j=0;
	    	jint_process_rows(cinfo);//在这里压缩
	    	memset((void*)(cinfo->inbuf),0,cinfo->inbuf_size);//清空输入缓冲区
	  	}
	}	
	jpeg_finish_compress(cinfo);
	f_close(&fileW);
	LEDOn(LED4);
	while(1);
}
Beispiel #14
0
void timer_init(void)
{
    /* init clock */
    ticks_per_ms = SystemCoreClock / 1000u;
    ticks_per_us = ticks_per_ms / 1000u;
    /* init all timers */
    for (int idx = 0; idx < NTIMERS; idx++) {
        /* disable it: */
        timer[idx].flags = 0;
    }
    /* enable systick timer: */
    if (SysTick_Config(ticks_per_ms)) /* set timer to trigger interrupt every 1 millisecond */
    {
        /* capture clock error */
        LEDOn(LED_ERR);
        while (1);
    }
    /* set highest priority: */
    NVIC_SetPriority(SysTick_IRQn, 0);
}
Beispiel #15
0
void timer_register(void (*timer_fn)(void), uint32_t period_ms) {
    /* find free timer: */
    int idx;
    for (idx = 0; idx < NTIMERS; idx++) {
        if ((timer[idx].flags & TIMER_FLAG_ENABLED) == 0) {
            break;
        }
    }
    if (idx >= NTIMERS) {
        /* capture error */
        LEDOn(LED_ERR);
        while (1);
    }
    /* setup the info struct: */
    timer[idx].period    = period_ms;
    timer[idx].countdown = period_ms - 1;
    timer[idx].timer_fn  = timer_fn;
    /* enable it: */
    timer[idx].flags = TIMER_FLAG_ENABLED;
}
Beispiel #16
0
void AllLEDOn() {
    uint8_t i;
    for (i = 0; i < 8; i++)
        LEDOn(i);
}
Beispiel #17
0
int main(void)
{
	#warning change clock hse_value to 25m	
	SYSTIM_Init();
	
	LEDInit(LED_ACT);
	LEDInit(LED_COM);
	LEDInit(LED_ERR);
	
	LEDOn(LED_ACT);
	LEDOff(LED_COM);
	LEDOff(LED_ERR);
	SYSTIM_DelayTms(1000);
	LEDOff(LED_ACT);	
	
//	USBD_Init(&USB_OTG_dev,
//				USB_OTG_FS_CORE_ID,
//				&USR_desc,
//				&USBD_CDC_cb,
//				&USR_cb);

	usart_init();
	UART_DMAInit();
	if (!CheckXCLK())
	{
		XCLK_ON();	
		SYSTIM_DelayTms(100);
	}
#ifdef USE_MT9V034
	MT9V034_Init();
#endif
#ifdef USE_MT9D111	
	MT9D111_Init();	
#endif	
	#warning already enable i2c when init camera
	//mpu6050_enable();	
	memset(Cam_Capture,'1',FULL_IMAGE_SIZE);	
	Cam_Capture[FULL_IMAGE_SIZE] = 's';
	Cam_Capture[FULL_IMAGE_SIZE + 1] = 't';	
	Cam_Capture[FULL_IMAGE_SIZE + 2] = 'p';	
	DCMI_CaptureCmd(ENABLE);	
	while(1)
	{
		int i=0;
#ifdef TEST_MPU6050
		bool res;
		res = I2C_ReadBytes(I2C2,data,MPU_ADDR, MPU_DATA, 14);
		if (!res)
		{
			LEDOn(LED_ERR);		
		}
		ax = (double)(data[1] + (data[0]<<8));
		ay = (double)(data[3] + (data[2]<<8));
		az = (double)(data[5] + (data[4]<<8));
		wx = (double)(data[9] + (data[8]<<8));
		wy = (double)(data[11] + (data[10]<<8));
		wz = (double)(data[13] + (data[12]<<8));					
		SYSTIM_DelayTms(100);
#endif		

		if (DCMI_Flag)
		{
			DCMI_Flag = 0;
			LEDToggle(LED_ACT);
//			SendImageDMA((uint32_t*)&Cam_Capture[0], FULL_IMAGE_SIZE);			
//			for (i=0;i<FULL_IMAGE_SIZE;i++)
//			{
//				Cam_Buffer[i] = Cam_Capture[i];
//			}
			VCP_DataTx(Cam_Capture,FULL_IMAGE_SIZE+3);
			SYSTIM_DelayTms(5000);
 			DCMI_CaptureCmd(ENABLE); 	
		}	
	}
}
void main(void)
#endif
{

  RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTD, ENABLE);

  /* Configure all unused PORT pins to low power consumption */

  PORT_StructInit(&PORT_InitStructure);
  PORT_InitStructure.PORT_Pin = (PORT_Pin_All & ~(PORT_Pin_10 | PORT_Pin_11 |
                                                  PORT_Pin_12 | PORT_Pin_13 |
                                                  PORT_Pin_14));
  PORT_Init(MDR_PORTD, &PORT_InitStructure);


  /* Configure PORTD pins 10..14 for output to switch LEDs on/off */

  PORT_InitStructure.PORT_Pin   = (PORT_Pin_10 | PORT_Pin_11 | PORT_Pin_12 |
                                   PORT_Pin_13 | PORT_Pin_14);
  PORT_InitStructure.PORT_OE    = PORT_OE_OUT;
  PORT_InitStructure.PORT_FUNC  = PORT_FUNC_PORT;
  PORT_InitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
  PORT_InitStructure.PORT_SPEED = PORT_SPEED_SLOW;

  PORT_Init(MDR_PORTD, &PORT_InitStructure);

  /* Consequently turn all three used LEDs on and off */

  LEDOn(LED1);
  Delay(4*BLINK_DELAY);
  LEDOff(LED1);
  Delay(4*BLINK_DELAY);
  LEDOn(LED2);
  Delay(4*BLINK_DELAY);
  LEDOff(LED2);
  Delay(4*BLINK_DELAY);
  LEDOn(LED3);
  Delay(4*BLINK_DELAY);
  LEDOff(LED3);
  Delay(4*BLINK_DELAY);

  /* Infinite loop that demonstrates different input clock sources using */

  while (1)
  {
    /* Set RST_CLK to default */
    RST_CLK_DeInit();
    RST_CLK_PCLKcmd(RST_CLK_PCLK_PORTD, ENABLE);

    /* 1. CPU_CLK = HSI clock */

    /* Enable HSI clock source */
    RST_CLK_HSIcmd(ENABLE);
    /* Switch LED2 on and wait for HSI ready status */
    LEDOn(LED2);
    Delay(BLINK_DELAY);
    if (RST_CLK_HSIstatus() == SUCCESS)                     /* Good HSI clock */
    {
      /* Switch LED2 off */
      LEDOff(LED2);
      /* Select HSI clock on the CPU clock MUX */
      RST_CLK_CPUclkSelection(RST_CLK_CPUclkHSI);
      /* LED1 blinking with HSI clock as input clock source */
      BlinkLED1(BLINK_NUM, BLINK_DELAY);
    }
    else                                                    /* HSI timeout */
    {
      IndicateError();
    }

    /* 2. CPU_CLK = HSI/2 clock */

    /* Enable HSI clock source */
    RST_CLK_HSIcmd(ENABLE);
    /* Disable CPU_PLL */
    RST_CLK_CPU_PLLcmd(DISABLE);
    /* Select HSI/2 clock as CPU_PLL input clock source */
    RST_CLK_CPU_PLLconfig(RST_CLK_CPU_PLLsrcHSIdiv2, 1);
    /* Switch LED2 on and wait for HSI ready status */
    LEDOn(LED2);
    Delay(BLINK_DELAY);
    if (RST_CLK_HSIstatus() == SUCCESS)                     /* Good HSI clock */
    {
      /* Switch LED2 off */
      LEDOff(LED2);
      /* Set CPU_C3_prescaler to 1 */
      RST_CLK_CPUclkPrescaler(RST_CLK_CPUclkDIV1);
      /* Switch CPU_C2_SEL to CPU_C1 clock instead of CPU_PLL output */
      RST_CLK_CPU_PLLuse(DISABLE);
      /* LED1 blinking with HSI/2 clock as input clock source */
      BlinkLED1(BLINK_NUM, BLINK_DELAY);
    }
    else                                                    /* HSI timeout */
    {
      IndicateError();
    }

    /* 3. CPU_CLK = 7*HSE/2 clock */

    /* Enable HSE clock oscillator */
    RST_CLK_HSEconfig(RST_CLK_HSE_ON);
    /* Switch LED2 on and wait for HSE ready status */
    LEDOn(LED2);
    Delay(BLINK_DELAY);
    if (RST_CLK_HSEstatus() == SUCCESS)                     /* Good HSE clock */
    {
      /* Select HSE clock as CPU_PLL input clock source */
      /* Set PLL multiplier to 7                        */
      RST_CLK_CPU_PLLconfig(RST_CLK_CPU_PLLsrcHSEdiv1, 7);
      /* Enable CPU_PLL */
      RST_CLK_CPU_PLLcmd(ENABLE);
      /* Switch LED2 on and wait for PLL ready status */
      if (RST_CLK_HSEstatus() == SUCCESS)                     /* Good CPU PLL */
      {
        /* Switch LED2 off */
        LEDOff(LED2);
        /* Set CPU_C3_prescaler to 2 */
        RST_CLK_CPUclkPrescaler(RST_CLK_CPUclkDIV2);
        /* Set CPU_C2_SEL to CPU_PLL output instead of CPU_C1 clock */
        RST_CLK_CPU_PLLuse(ENABLE);
        /* Select CPU_C3 clock on the CPU clock MUX */
        RST_CLK_CPUclkSelection(RST_CLK_CPUclkCPU_C3);
        /* LED1 blinking with 7*HSE/2 clock as input clock source */
        BlinkLED1(BLINK_NUM, BLINK_DELAY);
      }
      else                                                    /* CPU_PLL timeout */
      {
        IndicateError();
      }
    }
    else                                                    /* HSE timeout */
    {
      IndicateError();
    }

    /* 4. CPU_CLK = LSI clock */

    /* Enable LSI clock source */
    RST_CLK_LSIcmd(ENABLE);
    /* Switch LED2 on and wait for LSI ready status */
    LEDOn(LED2);
    Delay(BLINK_DELAY);
    if (RST_CLK_LSIstatus() == SUCCESS)                     /* Good LSI clock */
    {
      /* Switch LED2 off */
      LEDOff(LED2);
      /* Select LSI clock on the CPU clock MUX */
      RST_CLK_CPUclkSelection(RST_CLK_CPUclkLSI);
      /* LED1 blinking with LSI clock as input clock source */
      BlinkLED1(BLINK_NUM, BLINK_DELAY);
    }
    else                                                    /* LSI timeout */
    {
      IndicateError();
    }
  }
}
Beispiel #19
0
/**
  * @brief  Main function.
  */
int main(void)
{
	/* load settings and parameters */
	global_data_reset_param_defaults();
	global_data_reset();

	/* init led */
	LEDInit(LED_ACT);
	LEDInit(LED_COM);
	LEDInit(LED_ERR);
	LEDOff(LED_ACT);
	LEDOff(LED_COM);
	LEDOff(LED_ERR);

	/* enable FPU on Cortex-M4F core */
	SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */

	/* init clock */
	if (SysTick_Config(SystemCoreClock / 1000))
	{
		/* capture clock error */
		LEDOn(LED_ERR);
		while (1);
	}

	/* init usb */
	USBD_Init(	&USB_OTG_dev,
				USB_OTG_FS_CORE_ID,
				&USR_desc,
				&USBD_CDC_cb,
				&USR_cb);

	/* init mavlink */
	communication_init();

	/* enable image capturing */
	enable_image_capture();

	/* gyro config */
	gyro_config();

	/* init and clear fast image buffers */
	for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
	{
		image_buffer_8bit_1[i] = 0;
		image_buffer_8bit_2[i] = 0;
	}

	uint8_t * current_image = image_buffer_8bit_1;
	uint8_t * previous_image = image_buffer_8bit_2;

	/* usart config*/
	usart_init();

    /* i2c config*/
    i2c_init();

	/* sonar config*/
	float sonar_distance_filtered = 0.0f; // distance in meter
	float sonar_distance_raw = 0.0f; // distance in meter
	bool distance_valid = false;
	sonar_config();

	/* reset/start timers */
	timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
	timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
	timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
	timer[TIMER_PARAMS] = PARAMS_COUNT;
	timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];

	/* variables */
	uint32_t counter = 0;
	uint8_t qual = 0;

	/* bottom flow variables */
	float pixel_flow_x = 0.0f;
	float pixel_flow_y = 0.0f;
	float pixel_flow_x_sum = 0.0f;
	float pixel_flow_y_sum = 0.0f;
	float velocity_x_sum = 0.0f;
	float velocity_y_sum = 0.0f;
	float velocity_x_lp = 0.0f;
	float velocity_y_lp = 0.0f;
	int valid_frame_count = 0;
	int pixel_flow_count = 0;

	/* main loop */
	while (1)
	{
		/* reset flow buffers if needed */
		if(buffer_reset_needed)
		{
			buffer_reset_needed = 0;
			for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
			{
				image_buffer_8bit_1[i] = 0;
				image_buffer_8bit_2[i] = 0;
			}
			delay(500);
			continue;
		}

		/* calibration routine */
		if(global_data.param[PARAM_CALIBRATION_ON])
		{
			while(global_data.param[PARAM_CALIBRATION_ON])
			{
				dcmi_restart_calibration_routine();

				/* waiting for first quarter of image */
				while(get_frame_counter() < 2){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for second quarter of image */
				while(get_frame_counter() < 3){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for all image parts */
				while(get_frame_counter() < 4){}

				send_calibration_image(&previous_image, &current_image);

				if (global_data.param[PARAM_SYSTEM_SEND_STATE])
					communication_system_state_send();

				communication_receive_usb();
				debug_message_send_one();
				communication_parameter_send();

				LEDToggle(LED_COM);
			}

			dcmi_restart_calibration_routine();
			LEDOff(LED_COM);
		}

		uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];

		/* new gyroscope data */
		float x_rate_sensor, y_rate_sensor, z_rate_sensor;
		gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor);

		/* gyroscope coordinate transformation */
		float x_rate = y_rate_sensor; // change x and y rates
		float y_rate = - x_rate_sensor;
		float z_rate = z_rate_sensor; // z is correct

		/* calculate focal_length in pixel */
		const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled

		/* debug */
		float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
		float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px;

		//FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!!
//		x_rate = x_rate_raw_sensor; // change x and y rates
//		y_rate = y_rate_raw_sensor;

		/* get sonar data */
		sonar_read(&sonar_distance_filtered, &sonar_distance_raw);

		/* compute optical flow */
		if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
		{
			/* copy recent image to faster ram */
			dma_copy_image_buffers(&current_image, &previous_image, image_size, 1);

			/* compute optical flow */
			qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);

			if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
			{
				/* distances above 5m are considered invalid */
				sonar_distance_filtered = 0.0f;
				distance_valid = false;
			}
			else
			{
				distance_valid = true;
			}

			/*
			 * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
			 * x / f = X / Z
			 * y / f = Y / Z
			 */
			float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000.0f);
			float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f);

			/* integrate velocity and output values only if distance is valid */
			if (distance_valid)
			{
				/* calc velocity (negative of flow values scaled with distance) */
				float new_velocity_x = - flow_compx * sonar_distance_filtered;
				float new_velocity_y = - flow_compy * sonar_distance_filtered;

				if (qual > 0)
				{
					velocity_x_sum += new_velocity_x;
					velocity_y_sum += new_velocity_y;
					valid_frame_count++;

					/* lowpass velocity output */
					velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
							(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
					velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
							(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
				}
				else
				{
					/* taking flow as zero */
					velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
					velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
				}
			}
			else
			{
				/* taking flow as zero */
				velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
				velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
			}

			pixel_flow_x_sum += pixel_flow_x;
			pixel_flow_y_sum += pixel_flow_y;
			pixel_flow_count++;

		}

		counter++;

		/* TODO for debugging */
		//mavlink_msg_named_value_float_send(MAVLINK_COMM_2, boot_time_ms, "blabla", blabla);

		if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
		{
			/* send bottom flow if activated */
			if (counter % 2 == 0)
			{
				float flow_comp_m_x = 0.0f;
				float flow_comp_m_y = 0.0f;
				float ground_distance = 0.0f;
				if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
				{
					flow_comp_m_x = velocity_x_lp;
					flow_comp_m_y = velocity_y_lp;
				}
				else
				{
					flow_comp_m_x = velocity_x_sum/valid_frame_count;
					flow_comp_m_y = velocity_y_sum/valid_frame_count;
				}

				if(global_data.param[PARAM_SONAR_FILTERED])
					ground_distance = sonar_distance_filtered;
				else
					ground_distance = sonar_distance_raw;

				if (valid_frame_count > 0)
				{
					// send flow
					mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
							pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
							flow_comp_m_x, flow_comp_m_y, qual, ground_distance);

					if (global_data.param[PARAM_USB_SEND_FLOW])
						mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
							pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
							flow_comp_m_x, flow_comp_m_y, qual, ground_distance);

                    update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual,
                            ground_distance, x_rate, y_rate, z_rate);

				}
				else
				{
					// send distance
					mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
						pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
						0.0f, 0.0f, 0, ground_distance);

					if (global_data.param[PARAM_USB_SEND_FLOW])
						mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
							pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
							0.0f, 0.0f, 0, ground_distance);
	
                    update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate,
                            z_rate);
                }

				if(global_data.param[PARAM_USB_SEND_GYRO])
				{
					mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_ms() * 1000, x_rate, y_rate, z_rate);
				}

				velocity_x_sum = 0.0f;
				velocity_y_sum = 0.0f;
				pixel_flow_x_sum = 0.0f;
				pixel_flow_y_sum = 0.0f;
				valid_frame_count = 0;
				pixel_flow_count = 0;
			}
		}

		/* forward flow from other sensors */
		if (counter % 2)
		{
			communication_receive_forward();
		}

		/* send system state, receive commands */
		if (send_system_state_now)
		{
			/* every second */
			if (global_data.param[PARAM_SYSTEM_SEND_STATE])
			{
				communication_system_state_send();
			}
			send_system_state_now = false;
		}

		/* receive commands */
		if (receive_now)
		{
			/* test every second */
			communication_receive();
			communication_receive_usb();
			receive_now = false;
		}

		/* sending debug msgs and requested parameters */
		if (send_params_now)
		{
			debug_message_send_one();
			communication_parameter_send();
			send_params_now = false;
		}

		/*  transmit raw 8-bit image */
		if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
		{
			/* get size of image to send */
			uint16_t image_size_send;
			uint16_t image_width_send;
			uint16_t image_height_send;

			image_size_send = image_size;
			image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
			image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];

			if (global_data.param[PARAM_VIDEO_USB_MODE] == CAM_VIDEO)
			{
				mavlink_msg_data_transmission_handshake_send(
						MAVLINK_COMM_2,
						MAVLINK_DATA_STREAM_IMG_RAW8U,
						image_size_send,
						image_width_send,
						image_height_send,
						image_size_send / 253 + 1,
						253,
						100);
				LEDToggle(LED_COM);
				uint16_t frame;
				for (frame = 0; frame < image_size_send / 253 + 1; frame++)
				{
					mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * 253]);
				}

			}
			else if (global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO)
			{
				mavlink_msg_data_transmission_handshake_send(
						MAVLINK_COMM_2,
						MAVLINK_DATA_STREAM_IMG_RAW8U,
						image_size_send,
						image_width_send,
						image_height_send,
						image_size_send / 253 + 1,
						253,
						100);
				LEDToggle(LED_COM);
				uint16_t frame;
				for (frame = 0; frame < image_size / 253 + 1; frame++)
				{
					mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * 253]);
				}
			}
			send_image_now = false;
		}
		else if (!global_data.param[PARAM_USB_SEND_VIDEO])
		{
			LEDOff(LED_COM);
		}
	}
}
Beispiel #20
0
void SDCardReadWriteTextTask(void* parameters)
{
	uint8_t outbuf[512];
	uint8_t inbuf[512];
	int i;

	//LED_LowLevel_Init();
	//UART_LowLevel_Init();
	SD_LowLevel_Init();   //Initialize PINS, vector table and SDIO interface
	//UART_SendLine("Ready...Steady\n");

	printf("SSD\n\r");
	LEDOn(LED2);

	//Initialize the SD
	SD_Init();  //After return from this function sd is in transfer mode.
	//UART_SendLine("GO\n");

	printf("SDI\n\r");
	LEDOff(LED2);

	//Write a single block to the SD
	for (i=0;i<512;i++) {
		outbuf[i]='a';
	}

	//Single Block Test
	SD_WriteSingleBlock(outbuf, 3000);
	SD_ReadSingleBlock(inbuf, 3000);
	//UART_SendLine("First Block\n");
	//UART_Send(inbuf,512);
	printf("\n\r%s", "First Block");
	printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);

	SD_WriteSingleBlock(outbuf, 3001);
	SD_ReadSingleBlock(inbuf, 3001);
	//UART_SendLine("Second Block\n");
	//UART_Send(inbuf,512);
	printf("\n\r%s", "Second Block");
	printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);

	//Multiple block
	SD_StartMultipleBlockWrite(1000);
	SD_WriteData(outbuf,500);
	SD_WriteData(outbuf+50,50);
	SD_WriteData(outbuf,500);
	SD_StopMultipleBlockWrite();


	SD_ReadSingleBlock(inbuf, 1000);
	//UART_SendLine("Mult. Block 1\n");
	//UART_Send(inbuf,512);
	printf("\n\r%s", "Mult. Block 1");
	printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);
	SD_WaitTransmissionEnd();
	SD_ReadSingleBlock(inbuf, 1001);
	//UART_SendLine("Mult. Block 2\n");
	//UART_Send(inbuf,512);
	printf("\n\r%s", "Mult. Block 2");
	printf("\n\r%i,%i,%i,%i", (int)inbuf[0], (int)inbuf[1], (int)inbuf[2], (int)inbuf[3]);

//	LEDOn(LED6);

	portTickType lastExecutionTime = xTaskGetTickCount();
	for (;;)
	{
		DelayUntilTime( &lastExecutionTime, SDCardReadWriteTextTaskT);

	}
}
Beispiel #21
0
void task2(void * pdata)
{
    static bool  sb_channel_light =false;
    u8 buffer[40] = {0};
    while(1)
    {
        //printf("task2 ");
        Uart_PrintStr("task2 ");

        __enable_interrupt();

        if(g_flag__rx_frame_complete)//串口收到消息,字符串
        {
            g_flag__rx_frame_complete=false;
            //pBuf=UART2_GetData(&len);
            Uart_GetStr( (char *)g_uart_rx_content);
            if(strstr((char const *)g_uart_rx_content,"light") )
            {
                //printf("you send the message is :light \r\n");
                Uart_PrintStr("you send the message is :light \r\n");
                sb_channel_light = true;
            }
            else if(strstr((char const *)g_uart_rx_content,"bat"))
            {
                Uart_PrintStr("you send the message is :bat \r\n");
                sb_channel_light = false;
            }
            else if(strstr((char const *)g_uart_rx_content,"halt"))
            {
                Uart_PrintStr("you send the message is :halt,now halt the system! \r\n");
                asm("halt\n");
                // halt(); /* Program halted */
            }
        }

        static u8 voltageStr[20] = {0};
        static u16 temp_vol = 0;//8bit will overflow !! >255
        if(sb_channel_light)
        {
            temp_vol = (u16)GetADC(ADC_CHANNEL_LIGHT);

        }
        else
        {
            temp_vol = (u16)GetADC(ADC_CHANNEL_BATTERY);
        }
        voltageStr[0]= temp_vol/100 +'0';
        voltageStr[1] = '.';
        voltageStr[2]= temp_vol%100/10  +'0';
        voltageStr[3]= temp_vol%10  +'0';
        voltageStr[4]= '\0';
        if(sb_channel_light)
        {
            sprintf(buffer,"light: %s V\r\n",voltageStr);
            Uart_PrintStr(buffer);
            //printf("light: %s V\r\n",voltageStr);
        }
        else
        {
            //printf("battery: %s V\r\n",voltageStr);
            sprintf(buffer,"battery: %s V\r\n",voltageStr);
            Uart_PrintStr(buffer);
        }

        g_flag_awu =true;

#if 0
        if(g_flag_awu)//已退出睡眠
        {
            g_flag_awu = false;
            LEDOn(GREEN);
            Delay(0x3ffff);
            LEDOn(LED_OFF);
            asm("halt\n");
        }
#endif
        //      current_uart_handler(g_uart_rx_content, len);
        //      break;

        OSTimeDlyHMSM(0,0,0,TIME_DELAY-800 );
        //LEDOn(LED_OFF);
#if 0
        __disable_interrupt();
        //LED2_On();
        //LEDOn(LED_OFF);
        __enable_interrupt();
        OSTimeDlyHMSM(0,0,0,TIME_DELAY );
        __disable_interrupt();
        //LED2_Off();
        //LEDOn(LED_OFF);
        __enable_interrupt();
        OSTimeDlyHMSM(0,0,0,TIME_DELAY);
#endif
    }
}
//****************************************************************************
//
// This command allows setting, clearing or toggling the Status LED.
//
// The first argument should be one of the following:
// on     - Turn on the LED.
// off    - Turn off the LEd.
// toggle - Toggle the current LED status.
// activity - Set the LED mode to monitor serial activity.
//
//****************************************************************************
int
Cmd_led(int argc, char *argv[])
{
    //
    // These values only check the second character since all parameters are
    // different in that character.
    //
    if(argv[1][1] == 'n')
    {
        //
        // Turn on the LED.
        //
        LEDOn();

        //
        // Switch off activity mode.
        //
        HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
    }
    else if(argv[1][1] == 'f')
    {
        //
        // Turn off the LED.
        //
        LEDOff();

        //
        // Switch off activity mode.
        //
        HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
    }
    else if(argv[1][1] == 'o')
    {
        //
        // Toggle the LED.
        //
        LEDToggle();

        //
        // Switch off activity mode.
        //
        HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 0;
    }
    else if(argv[1][1] == 'c')
    {
        //
        // If this is the "activity" value then set the activity mode.
        //
        HWREGBITW(&g_ulFlags, FLAG_LED_ACTIVITY) = 1;
    }
    else
    {
        //
        // The command format was not correct so print out some help.
        //
        CommandPrint("\nled <on|off|toggle|activity>\n");
        CommandPrint("  on       - Turn on the LED.\n");
        CommandPrint("  off      - Turn off the LED.\n");
        CommandPrint("  toggle   - Toggle the LED state.\n");
        CommandPrint("  activity - LED state will toggle on UART activity.\n");
    }

    return(0);
}
//****************************************************************************
//
// Handles CDC driver notifications related to control and setup of the
// device.
//
// \param pvCBData is the client-supplied callback pointer for this channel.
// \param ulEvent identifies the event we are being notified about.
// \param ulMsgValue is an event-specific value.
// \param pvMsgData is an event-specific pointer.
//
// This function is called by the CDC driver to perform control-related
// operations on behalf of the USB host.  These functions include setting
// and querying the serial communication parameters, setting handshake line
// states and sending break conditions.
//
// \return The return value is event-specific.
//
//****************************************************************************
unsigned long
SerialHandler(void *pvCBData, unsigned long ulEvent, unsigned long ulMsgValue,
              void *pvMsgData)
{
    tLineCoding *psLineCoding;

    //
    // Which event are we being asked to process?
    //
    switch(ulEvent)
    {
        //
        // We are connected to a host and communication is now possible.
        //
        case USB_EVENT_CONNECTED:
        {
            //
            // Flush our buffers.
            //
            USBBufferFlush(&g_sTxBuffer);
            USBBufferFlush(&g_sRxBuffer);

            //
            // Turn on the user LED.
            //
            LEDOn();

            break;
        }

        case USB_EVENT_DISCONNECTED:
        {
            //
            // Turn off the user LED.
            //
            LEDOff();

            break;
        }

        //
        // Return the current serial communication parameters.
        //
        case USBD_CDC_EVENT_GET_LINE_CODING:
        {
            //
            // Create a pointer to the line coding information.
            //
            psLineCoding = (tLineCoding *)pvMsgData;

            //
            // Copy the current line coding information into the structure.
            //
            psLineCoding->ulRate = g_sLineCoding.ulRate;
            psLineCoding->ucStop = g_sLineCoding.ucStop;
            psLineCoding->ucParity = g_sLineCoding.ucParity;
            psLineCoding->ucDatabits = g_sLineCoding.ucDatabits;
            break;
        }

        //
        // Set the current serial communication parameters.
        //
        case USBD_CDC_EVENT_SET_LINE_CODING:
        {
            //
            // Create a pointer to the line coding information.
            //
            psLineCoding = (tLineCoding *)pvMsgData;

            //
            // Copy the line coding information into the current line coding
            // structure.
            //
            g_sLineCoding.ulRate = psLineCoding->ulRate;
            g_sLineCoding.ucStop = psLineCoding->ucStop;
            g_sLineCoding.ucParity = psLineCoding->ucParity;
            g_sLineCoding.ucDatabits = psLineCoding->ucDatabits;
            break;
        }

        //
        // Set the current serial communication parameters.
        //
        case USBD_CDC_EVENT_SET_CONTROL_LINE_STATE:
        {
            break;
        }

        //
        // All other events are ignored.
        //
        default:
        {
            break;
        }
    }

    return(0);
}
Beispiel #24
0
Datei: main.c Projekt: PX4/Flow
/**
  * @brief  Main function.
  */
int main(void)
{
	__enable_irq();

	/* load settings and parameters */
	global_data_reset_param_defaults();
	global_data_reset();
	PROBE_INIT();
	/* init led */
	LEDInit(LED_ACT);
	LEDInit(LED_COM);
	LEDInit(LED_ERR);
	LEDOff(LED_ACT);
	LEDOff(LED_COM);
	LEDOff(LED_ERR);
        board_led_rgb(255,255,255, 1);
        board_led_rgb(  0,  0,255, 0);
        board_led_rgb(  0,  0, 0, 0);
        board_led_rgb(255,  0,  0, 1);
        board_led_rgb(255,  0,  0, 2);
        board_led_rgb(255,  0,  0, 3);
                board_led_rgb(  0,255,  0, 3);
        board_led_rgb(  0,  0,255, 4);

	/* enable FPU on Cortex-M4F core */
	SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */

	/* init clock */
	if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
	{
		/* capture clock error */
		LEDOn(LED_ERR);
		while (1);
	}

	/* init usb */
	USBD_Init(	&USB_OTG_dev,
				USB_OTG_FS_CORE_ID,
				&USR_desc,
				&USBD_CDC_cb,
				&USR_cb);

	/* init mavlink */
	communication_init();

	/* enable image capturing */
	enable_image_capture();

	/* gyro config */
	gyro_config();

	/* init and clear fast image buffers */
	for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
	{
		image_buffer_8bit_1[i] = 0;
		image_buffer_8bit_2[i] = 0;
	}

	uint8_t * current_image = image_buffer_8bit_1;
	uint8_t * previous_image = image_buffer_8bit_2;

	/* usart config*/
	usart_init();

    /* i2c config*/
    i2c_init();

	/* sonar config*/
	float sonar_distance_filtered = 0.0f; // distance in meter
	float sonar_distance_raw = 0.0f; // distance in meter
	bool distance_valid = false;
	sonar_config();

	/* reset/start timers */
	timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
	timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
	timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
	timer[TIMER_PARAMS] = PARAMS_COUNT;
	timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];

	/* variables */
	uint32_t counter = 0;
	uint8_t qual = 0;

	/* bottom flow variables */
	float pixel_flow_x = 0.0f;
	float pixel_flow_y = 0.0f;
	float pixel_flow_x_sum = 0.0f;
	float pixel_flow_y_sum = 0.0f;
	float velocity_x_sum = 0.0f;
	float velocity_y_sum = 0.0f;
	float velocity_x_lp = 0.0f;
	float velocity_y_lp = 0.0f;
	int valid_frame_count = 0;
	int pixel_flow_count = 0;

	static float accumulated_flow_x = 0;
	static float accumulated_flow_y = 0;
	static float accumulated_gyro_x = 0;
	static float accumulated_gyro_y = 0;
	static float accumulated_gyro_z = 0;
	static uint16_t accumulated_framecount = 0;
	static uint16_t accumulated_quality = 0;
	static uint32_t integration_timespan = 0;
	static uint32_t lasttime = 0;
	uint32_t time_since_last_sonar_update= 0;
	uint32_t time_last_pub= 0;

	uavcan_start();
	/* main loop */
	while (1)
	{
                PROBE_1(false);
                uavcan_run();
                PROBE_1(true);
		/* reset flow buffers if needed */
		if(buffer_reset_needed)
		{
			buffer_reset_needed = 0;
			for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
			{
				image_buffer_8bit_1[i] = 0;
				image_buffer_8bit_2[i] = 0;
			}
			delay(500);
			continue;
		}

		/* calibration routine */
		if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
		{
			while(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
			{
				dcmi_restart_calibration_routine();

				/* waiting for first quarter of image */
				while(get_frame_counter() < 2){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for second quarter of image */
				while(get_frame_counter() < 3){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for all image parts */
				while(get_frame_counter() < 4){}

				send_calibration_image(&previous_image, &current_image);

				if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
					communication_system_state_send();

				communication_receive_usb();
				debug_message_send_one();
				communication_parameter_send();

				LEDToggle(LED_COM);
			}

			dcmi_restart_calibration_routine();
			LEDOff(LED_COM);
		}

		uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];

		/* new gyroscope data */
		float x_rate_sensor, y_rate_sensor, z_rate_sensor;
		int16_t gyro_temp;
		gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);

		/* gyroscope coordinate transformation */
		float x_rate = y_rate_sensor; // change x and y rates
		float y_rate = - x_rate_sensor;
		float z_rate = z_rate_sensor; // z is correct

		/* calculate focal_length in pixel */
		const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled

		/* get sonar data */
		distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);

		/* reset to zero for invalid distances */
		if (!distance_valid) {
			sonar_distance_filtered = 0.0f;
			sonar_distance_raw = 0.0f;
		}

		/* compute optical flow */
		if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
		{
			/* copy recent image to faster ram */
			dma_copy_image_buffers(&current_image, &previous_image, image_size, 1);

			/* compute optical flow */
			qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);

			/*
			 * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
			 * x / f = X / Z
			 * y / f = Y / Z
			 */
			float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
			float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);

			if (qual > 0)
			{
				valid_frame_count++;

				uint32_t deltatime = (get_boot_time_us() - lasttime);
				integration_timespan += deltatime;
				accumulated_flow_x += pixel_flow_y  / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
				accumulated_flow_y += pixel_flow_x  / focal_length_px * -1.0f;//rad
				accumulated_gyro_x += x_rate * deltatime / 1000000.0f;	//rad
				accumulated_gyro_y += y_rate * deltatime / 1000000.0f;	//rad
				accumulated_gyro_z += z_rate * deltatime / 1000000.0f;	//rad
				accumulated_framecount++;
				accumulated_quality += qual;
			}


			/* integrate velocity and output values only if distance is valid */
			if (distance_valid)
			{
				/* calc velocity (negative of flow values scaled with distance) */
				float new_velocity_x = - flow_compx * sonar_distance_filtered;
				float new_velocity_y = - flow_compy * sonar_distance_filtered;

				time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());

				if (qual > 0)
				{
					velocity_x_sum += new_velocity_x;
					velocity_y_sum += new_velocity_y;

					/* lowpass velocity output */
					velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
							(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
					velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
							(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
				}
				else
				{
					/* taking flow as zero */
					velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
					velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
				}
			}
			else
			{
				/* taking flow as zero */
				velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
				velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
			}
			//update lasttime
			lasttime = get_boot_time_us();

			pixel_flow_x_sum += pixel_flow_x;
			pixel_flow_y_sum += pixel_flow_y;
			pixel_flow_count++;

		}

		counter++;

		if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
		{
			/* send bottom flow if activated */

			float ground_distance = 0.0f;


			if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]))
			{
				ground_distance = sonar_distance_filtered;
			}
			else
			{
				ground_distance = sonar_distance_raw;
			}

                        uavcan_define_export(i2c_data, legacy_12c_data_t, ccm);
                        uavcan_define_export(range_data, range_data_t, ccm);
			uavcan_timestamp_export(i2c_data);
                        uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc);
			//update I2C transmitbuffer
			if(valid_frame_count>0)
			{
				update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
						ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
			}
			else
			{
				update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual,
						ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
			}
	                PROBE_2(false);
                        uavcan_publish(range, 40, range_data);
	                PROBE_2(true);

                        PROBE_3(false);
                        uavcan_publish(flow, 40, i2c_data);
                        PROBE_3(true);

            //serial mavlink  + usb mavlink output throttled
			uint32_t now = get_boot_time_us();
			uint32_t time_since_last_pub = now - time_last_pub;
			if (time_since_last_pub > (1.0e6f/global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE]))
			{
				time_last_pub = now;

				float flow_comp_m_x = 0.0f;
				float flow_comp_m_y = 0.0f;

				if(FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED]))
				{
					flow_comp_m_x = velocity_x_lp;
					flow_comp_m_y = velocity_y_lp;
				}
				else
				{
					if(valid_frame_count>0)
					{
						flow_comp_m_x = velocity_x_sum/valid_frame_count;
						flow_comp_m_y = velocity_y_sum/valid_frame_count;
					}
					else
					{
						flow_comp_m_x = 0.0f;
						flow_comp_m_y = 0.0f;
					}
				}


				// send flow
				mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
						pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
						flow_comp_m_x, flow_comp_m_y, qual, ground_distance);

				mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
						integration_timespan, accumulated_flow_x, accumulated_flow_y,
						accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
						gyro_temp, accumulated_quality/accumulated_framecount,
						time_since_last_sonar_update,ground_distance);

				/* send approximate local position estimate without heading */
				if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
				{
					/* rough local position estimate for unit testing */
					lpos.x += ground_distance*accumulated_flow_x;
					lpos.y += ground_distance*accumulated_flow_y;
					lpos.z = -ground_distance;
					lpos.vx = ground_distance*accumulated_flow_x/integration_timespan;
					lpos.vy = ground_distance*accumulated_flow_y/integration_timespan;
					lpos.vz = 0; // no direct measurement, just ignore

				} else {
					/* toggling param allows user reset */
					lpos.x = 0;
					lpos.y = 0;
					lpos.z = 0;
					lpos.vx = 0;
					lpos.vy = 0;
					lpos.vz = 0;
				}

				if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_FLOW]))
				{
					mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
							pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
						flow_comp_m_x, flow_comp_m_y, qual, ground_distance);


					mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
							integration_timespan, accumulated_flow_x, accumulated_flow_y,
							accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
							gyro_temp, accumulated_quality/accumulated_framecount,
							time_since_last_sonar_update,ground_distance);
				}


				if(FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_GYRO]))
				{
					mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
				}

				integration_timespan = 0;
				accumulated_flow_x = 0;
				accumulated_flow_y = 0;
				accumulated_framecount = 0;
				accumulated_quality = 0;
				accumulated_gyro_x = 0;
				accumulated_gyro_y = 0;
				accumulated_gyro_z = 0;

				velocity_x_sum = 0.0f;
				velocity_y_sum = 0.0f;
				pixel_flow_x_sum = 0.0f;
				pixel_flow_y_sum = 0.0f;
				valid_frame_count = 0;
				pixel_flow_count = 0;
			}
		}

		/* forward flow from other sensors */
		if (counter % 2)
		{
			communication_receive_forward();
		}

		/* send system state, receive commands */
		if (send_system_state_now)
		{
			/* every second */
			if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
			{
				communication_system_state_send();
			}
			send_system_state_now = false;
		}

		/* receive commands */
		if (receive_now)
		{
			/* test every second */
			communication_receive();
			communication_receive_usb();
			receive_now = false;
		}

		/* sending debug msgs and requested parameters */
		if (send_params_now)
		{
			debug_message_send_one();
			communication_parameter_send();
			send_params_now = false;
		}

		/* send local position estimate, for testing only, doesn't account for heading */
		if (send_lpos_now)
		{
			if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
			{
				mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
			}
			send_lpos_now = false;
		}

		/*  transmit raw 8-bit image */
		if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])&& send_image_now)
		{
			/* get size of image to send */
			uint16_t image_size_send;
			uint16_t image_width_send;
			uint16_t image_height_send;

			image_size_send = image_size;
			image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
			image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];

			mavlink_msg_data_transmission_handshake_send(
					MAVLINK_COMM_2,
					MAVLINK_DATA_STREAM_IMG_RAW8U,
					image_size_send,
					image_width_send,
					image_height_send,
					image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
					MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
					100);
			LEDToggle(LED_COM);
			uint16_t frame = 0;
			for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
			{
				mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
			}

			send_image_now = false;
		}
		else if (!FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]))
		{
			LEDOff(LED_COM);
		}
	}
}
Beispiel #25
0
/**
  * @brief  Main function.
  */
int main(void)
{
	/* load settings and parameters */
	global_data_reset_param_defaults();
	global_data_reset();

	/* init led */
	LEDInit(LED_ACT);
	LEDInit(LED_COM);
	LEDInit(LED_ERR);
	LEDOff(LED_ACT);
	LEDOff(LED_COM);
	LEDOff(LED_ERR);

	/* enable FPU on Cortex-M4F core */
	SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */

	/* init clock */
	if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
	{
		/* capture clock error */
		LEDOn(LED_ERR);
		while (1);
	}

	/* init usb */
	USBD_Init(	&USB_OTG_dev,
				USB_OTG_FS_CORE_ID,
				&USR_desc,
				&USBD_CDC_cb,
				&USR_cb);

	/* init mavlink */
	communication_init();

	/* enable image capturing */
	enable_image_capture();

	/* gyro config */
	gyro_config();

	/* init and clear fast image buffers */
	for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
	{
		image_buffer_8bit_1[i] = 0;
		image_buffer_8bit_2[i] = 0;
	}

	uint8_t * current_image = image_buffer_8bit_1;
	uint8_t * previous_image = image_buffer_8bit_2;
    
    uint8_t   current_image1[64][64];
    uint8_t   current_image2[64][64];
    uint8_t   current_image3[64][64];
    uint8_t   current_image4[64][64];
    uint8_t   previous_image1[64][64];
    uint8_t   previous_image2[64][64];
    uint8_t   previous_image3[64][64];
    uint8_t   previous_image4[64][64];
    for(int i=0;i<64;++i)
    {
        for(int j=0;j<64;++j)
        {
            current_image1[i][j]=0;
            previous_image1[i][j]=0;
            current_image2[i][j]=0;
            previous_image2[i][j]=0;
            current_image3[i][j]=0;
            previous_image3[i][j]=0;
            current_image4[i][j]=0;
            previous_image4[i][j]=0;
        }
    }
    
    uint8_t ourImage[OurSize];
    for(int i=0;i<OurSize;++i)
    {
        ourImage[i]=0;
    }
    
	/* usart config*/
	usart_init();

    /* i2c config*/
    i2c_init();

	/* sonar config*/
	float sonar_distance_filtered = 0.0f; // distance in meter
	float sonar_distance_raw = 0.0f; // distance in meter
	bool distance_valid = false;
	sonar_config();

	/* reset/start timers */
	timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
	timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
	timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
	timer[TIMER_PARAMS] = PARAMS_COUNT;
	timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];

	/* variables */
	uint32_t counter = 0;
	uint8_t qual = 0;

	/* bottom flow variables */
	float pixel_flow_x = 0.0f;
	float pixel_flow_y = 0.0f;
	float pixel_flow_x_sum = 0.0f;
	float pixel_flow_y_sum = 0.0f;
	float velocity_x_sum = 0.0f;
	float velocity_y_sum = 0.0f;
	float velocity_x_lp = 0.0f;
	float velocity_y_lp = 0.0f;
	int valid_frame_count = 0;
	int pixel_flow_count = 0;
	float pixel_flow_x_lp = 0.0f;
	float pixel_flow_y_lp = 0.0f;

	static float accumulated_flow_x = 0;
	static float accumulated_flow_y = 0;
	static float accumulated_gyro_x = 0;
	static float accumulated_gyro_y = 0;
	static float accumulated_gyro_z = 0;
	static uint16_t accumulated_framecount = 0;
	static uint16_t accumulated_quality = 0;
	static uint32_t integration_timespan = 0;
	static uint32_t lasttime = 0;
	uint32_t time_since_last_sonar_update= 0;

	static float last_vel_x = 0;
	static float last_vel_y = 0;
	float vel_x = 0, vel_y = 0;

    //Change
    int count=1;
    //int gyroCount=1;
    uint8_t * tmp_image1 = *previous_image;
    uint8_t * tmp_image2 = *current_image;
    float x_temprate=0;
    float y_temprate=0;
    float z_temprate=0;
    
    int defCount=3;
    
    
    
    for (uint16_t pixel = 0; pixel < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; pixel++)
    {
        *(tmp_image1+pixel)=0;
        *(tmp_image2+pixel)=0;
    }
    uint16_t image_sum1=0;
    uint16_t image_sum2=0;
    uint16_t image_sum3=0;
    uint16_t image_sum4=0;
    //EndChange


	/* main loop */
	while (1)
	{
        
        
        uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
        
		/* reset flow buffers if needed */
		if(buffer_reset_needed)
		{
			buffer_reset_needed = 0;
			for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
			{
				image_buffer_8bit_1[i] = 0;
				image_buffer_8bit_2[i] = 0;
			}
			delay(500);
			continue;
		}

		/* calibration routine */
		if(global_data.param[PARAM_VIDEO_ONLY])
		{
			while(global_data.param[PARAM_VIDEO_ONLY])
			{
				dcmi_restart_calibration_routine();

				/* waiting for first quarter of image */
				while(get_frame_counter() < 2){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for second quarter of image */
				while(get_frame_counter() < 3){}
				dma_copy_image_buffers(&current_image, &previous_image, FULL_IMAGE_SIZE, 1);

				/* waiting for all image parts */
				while(get_frame_counter() < 4){}

				send_calibration_image(&previous_image, &current_image);

				if (global_data.param[PARAM_SYSTEM_SEND_STATE])
					communication_system_state_send();

				communication_receive_usb();
				debug_message_send_one();
				communication_parameter_send();

				LEDToggle(LED_COM);
			}

			dcmi_restart_calibration_routine();
			LEDOff(LED_COM);
		}

        
        //StartChange
        if(count<=defCount)
        {
            if(count==1)
            {
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    image_sum1+=*(previous_image+pixel);
                }
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    *(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
                }
            }
            else
            {
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    image_sum2+=*(previous_image+pixel);
                }
                if(image_sum1<image_sum2)
                {
                    image_sum1=image_sum2;
                    for (uint16_t pixel = 0; pixel < image_size; pixel++)
                    {
                        *(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
                    }
                }
                image_sum2=0;
            }
        }
        if(count>defCount&&count<=2*defCount)
        {
//            uint16_t image_sum1=0;
//            uint16_t image_sum2=0;
            if(count==defCount+1)
            {
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    image_sum3+=*(current_image+pixel);
                }
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    *(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
                }
            }
            else
            {
                for (uint16_t pixel = 0; pixel < image_size; pixel++)
                {
                    image_sum4+=*(current_image+pixel);
                }
                if(image_sum3<image_sum4)
                {
                    image_sum3=image_sum4;
                    for (uint16_t pixel = 0; pixel < image_size; pixel++)
                    {
                        *(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
                    }
                }
                image_sum4=0;
            }
        }
//        if(count<=defCount)
//        {
//            for (uint16_t pixel = 0; pixel < image_size; pixel++)
//            {
//                *(tmp_image1+pixel)+=(uint8_t)(*(previous_image+pixel)/defCount);
//            }
//        }
//        if(count>defCount&&count<=2*defCount)
//        {
//            for (uint16_t pixel = 0; pixel < image_size; pixel++)
//            {
//                *(tmp_image2+pixel)+=(uint8_t)(*(current_image+pixel)/defCount);
//            }
//           
//        }
        count++;
        if(count==2*defCount+1)
        {
            
            /* new gyroscope data */
            float x_rate_sensor, y_rate_sensor, z_rate_sensor;
            int16_t gyro_temp;
            gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
            
            /* gyroscope coordinate transformation */
            x_temprate += y_rate_sensor/(2*defCount); // change x and y rates
            y_temprate += - x_rate_sensor/(2*defCount);
            z_temprate += z_rate_sensor/(2*defCount); // z is correct
            
            /* calculate focal_length in pixel */
            const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
            
            /* get sonar data */
            distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
            
            /* reset to zero for invalid distances */
            if (!distance_valid) {
                sonar_distance_filtered = 0.0f;
                sonar_distance_raw = 0.0f;
            }
            
            float x_rate = x_temprate; // change x and y rates
            float y_rate = - y_temprate;
            float z_rate = z_temprate; // z is correct
            x_temprate =0.0f;
            y_temprate =0.0f;
            z_temprate =0.0f;
            
//            /* calculate focal_length in pixel */
//            const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
//            
//            /* get sonar data */
//            distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
            
            
            *previous_image=tmp_image1;
            *current_image=tmp_image2;
            count=1;
            for (uint16_t pixel = 0; pixel < image_size; pixel++)
            {
                *(tmp_image1+pixel)=0;
                *(tmp_image2+pixel)=0;
            }
            image_sum1=0;
            image_sum2=0;
            image_sum3=0;
            image_sum4=0;
            /* compute optical flow */
            if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
            {
                /* copy recent image to faster ram */
                dma_copy_image_buffers(&current_image, &previous_image, image_size, 1);
                
                for(int i=0;i<64;++i)
                {
                    for(int j=0;j<64;++j)
                    {
                        previous_image1[i][j]=*(previous_image+64*i+j);
                        current_image1[i][j]=*(current_image+64*i+j);
                    }
                }
                
                /* compute optical flow */
                qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
                
                /*
                 * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
                 * x / f = X / Z
                 * y / f = Y / Z
                 */
                float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
                float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
                
                /* integrate velocity and output values only if distance is valid */
                if (distance_valid)
                {
                    /* calc velocity (negative of flow values scaled with distance) */
                    float new_velocity_x = - flow_compx * sonar_distance_filtered;
                    float new_velocity_y = - flow_compy * sonar_distance_filtered;
                    
                    time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());
                    
                    if (qual > 0)
                    {
                        velocity_x_sum += new_velocity_x;
                        velocity_y_sum += new_velocity_y;
                        valid_frame_count++;
                        
                        uint32_t deltatime = (get_boot_time_us() - lasttime);
                        integration_timespan += deltatime;
                        accumulated_flow_x += pixel_flow_y  / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
                        accumulated_flow_y += pixel_flow_x  / focal_length_px * -1.0f;//rad
                        accumulated_gyro_x += x_rate * deltatime / 1000000.0f;	//rad
                        accumulated_gyro_y += y_rate * deltatime / 1000000.0f;	//rad
                        accumulated_gyro_z += z_rate * deltatime / 1000000.0f;	//rad
                        accumulated_framecount++;
                        accumulated_quality += qual;
                        
                        /* lowpass velocity output */
                        velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
                        (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
                        velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
                        (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
                    }
                    else
                    {
                        /* taking flow as zero */
                        velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
                        velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
                    }
                }
                else
                {
                    /* taking flow as zero */
                    velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
                    velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
                }
                //update lasttime
                lasttime = get_boot_time_us();
                
                pixel_flow_x_sum += pixel_flow_x;
                pixel_flow_y_sum += pixel_flow_y;
                pixel_flow_count++;
                
            }
            
            counter++;
            
            if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
            {
                /* send bottom flow if activated */
                
                float ground_distance = 0.0f;
                
                
                if(global_data.param[PARAM_SONAR_FILTERED])
                {
                    ground_distance = sonar_distance_filtered;
                }
                else
                {
                    ground_distance = sonar_distance_raw;
                }
                
                if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
                {
                    /* lowpass velocity output */
                    pixel_flow_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_x +
                    (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_x_lp;
                    pixel_flow_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_y +
                    (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_y_lp;
                }
                else
                {
                    pixel_flow_x_lp = pixel_flow_x;
                    pixel_flow_y_lp = pixel_flow_y;
                }
                //update I2C transmitbuffer
                if(valid_frame_count>0)
                {
                    update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
                                     ground_distance, x_rate, y_rate, z_rate, gyro_temp);
                    last_vel_x = velocity_x_sum/valid_frame_count;
                    last_vel_y = velocity_y_sum/valid_frame_count;
                    vel_x = last_vel_x;
                    vel_y = last_vel_y;
                }
                else
                {
                    /*
                     update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, last_vel_x, last_vel_y, qual,
                     ground_distance, x_rate, y_rate, z_rate, gyro_temp);
                     vel_x = 0;
                     vel_y = 0;
                     */
                }
                
                //serial mavlink  + usb mavlink output throttled
                if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
                {
                    float flow_comp_m_x = 0.0f;
                    float flow_comp_m_y = 0.0f;
                    
                    if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
                    {
                        flow_comp_m_x = velocity_x_lp;
                        flow_comp_m_y = velocity_y_lp;
                    }
                    else
                    {
                        if(valid_frame_count>0)
                        {
                            flow_comp_m_x = velocity_x_sum/valid_frame_count;
                            flow_comp_m_y = velocity_y_sum/valid_frame_count;
                        }
                        else
                        {
                            flow_comp_m_x = 0.0f;
                            flow_comp_m_y = 0.0f;
                        }
                    }
                    
                    
                    // send flow
                    mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                                                  pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
                                                  flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
                    
                    
                    mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                                                      integration_timespan, accumulated_flow_x, accumulated_flow_y,
                                                      accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
                                                      gyro_temp, accumulated_quality/accumulated_framecount,
                                                      time_since_last_sonar_update,ground_distance);
                    
                    /*
                     mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                     integration_timespan, accumulated_flow_x, accumulated_flow_y,
                     vel_x, vel_y, accumulated_gyro_z,
                     gyro_temp, accumulated_quality/accumulated_framecount,
                     time_since_last_sonar_update,ground_distance);
                     */
                    /*
                     mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                     integration_timespan, accumulated_flow_x, accumulated_flow_y,
                     last_vel_x, last_vel_y, accumulated_gyro_z,
                     gyro_temp, accumulated_quality/accumulated_framecount,
                     time_since_last_sonar_update,ground_distance);
                     */
                    
                    
                    /* send approximate local position estimate without heading */
                    if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
                    {
                        /* rough local position estimate for unit testing */
                        lpos.x += ground_distance*accumulated_flow_x;
                        lpos.y += ground_distance*accumulated_flow_y;
                        lpos.z = -ground_distance;
                        /* velocity not directly measured and not important for testing */
                        lpos.vx = 0;
                        lpos.vy = 0;
                        lpos.vz = 0;
                        
                    } else {
                        /* toggling param allows user reset */
                        lpos.x = 0;
                        lpos.y = 0;
                        lpos.z = 0;
                        lpos.vx = 0;
                        lpos.vy = 0;
                        lpos.vz = 0;
                    }
                    
                    if (global_data.param[PARAM_USB_SEND_FLOW])
                    {
                        mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                                                      pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
                                                      flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
                        
                        
                        mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
                                                          integration_timespan, accumulated_flow_x, accumulated_flow_y,
                                                          accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
                                                          gyro_temp, accumulated_quality/accumulated_framecount,
                                                          time_since_last_sonar_update,ground_distance);
                    }
                    
                    
                    if(global_data.param[PARAM_USB_SEND_GYRO])
                    {
                        mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
                    }
                    
                    integration_timespan = 0;
                    accumulated_flow_x = 0;
                    accumulated_flow_y = 0;
                    accumulated_framecount = 0;
                    accumulated_quality = 0;
                    accumulated_gyro_x = 0;
                    accumulated_gyro_y = 0;
                    accumulated_gyro_z = 0;
                    
                    velocity_x_sum = 0.0f;
                    velocity_y_sum = 0.0f;
                    pixel_flow_x_sum = 0.0f;
                    pixel_flow_y_sum = 0.0f;
                    valid_frame_count = 0;
                    pixel_flow_count = 0;
                }
            }
            
            /* forward flow from other sensors */
            if (counter % 2)
            {
                communication_receive_forward();
            }
            
            /* send system state, receive commands */
            if (send_system_state_now)
            {
                /* every second */
                if (global_data.param[PARAM_SYSTEM_SEND_STATE])
                {
                    communication_system_state_send();
                }
                send_system_state_now = false;
            }
            
            /* receive commands */
            if (receive_now)
            {
                /* test every second */
                communication_receive();
                communication_receive_usb();
                receive_now = false;
            }
            
            /* sending debug msgs and requested parameters */
            if (send_params_now)
            {
                debug_message_send_one();
                communication_parameter_send();
                send_params_now = false;
            }
            
            /* send local position estimate, for testing only, doesn't account for heading */
            if (send_lpos_now)
            {
                if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
                {
                    mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
                }
                send_lpos_now = false;
            }
            
            /*  transmit raw 8-bit image */
            if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
            {
                /* get size of image to send */
                uint16_t image_size_send;
                uint16_t image_width_send;
                uint16_t image_height_send;
                
                image_size_send = image_size;
                image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
                image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
                
                mavlink_msg_data_transmission_handshake_send(
                                                             MAVLINK_COMM_2,
                                                             MAVLINK_DATA_STREAM_IMG_RAW8U,
                                                             image_size_send,
                                                             image_width_send,
                                                             image_height_send,
                                                             image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
                                                             MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
                                                             100);
                LEDToggle(LED_COM);
                uint16_t frame = 0;
                for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
                {
                    mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
                }
                
                send_image_now = false;
            }
            else if (!global_data.param[PARAM_USB_SEND_VIDEO])
            {
                LEDOff(LED_COM);
            }
        }
    
        
        //EndChange
	}
}
Beispiel #26
0
void main(void)
{
/******************************************************************************/
// Init
/******************************************************************************/
  WDTCTL = WDTPW + WDTHOLD;     // watchdog timer off
  configureClocks();
  init_PWM_TimerA();

  hour = 6;
  hour24 = 6;
  minute = 1;					// Set to one because with 0 no LED is on which is confusing.
  second = 0;
  state = SET_HOUR;
  capsense=0;
  LEDState=OFF;
  timer=0;
  DEMOPointer = 1;

  P1REN = BIT6 + BIT7;          // P1REN = pullup/down enable
  P1OUT = BIT6 + BIT7;          // P1OUT = define pullup
  P1IFG = 0;
  P1IES = BIT6 + BIT7;          // set high to low transition for P1.6&7 interrupt
  P1IE  = BIT6 + BIT7;          // enable P1.6&7 interrupt

  P1REN |= BIT4 + BIT5;         // P1REN = pullup/down enable
  P1OUT |= BIT4 + BIT5;         // P1OUT = pullup
  P2REN |= BIT0;                // P2REN = pullup/down enable
  P2OUT |= BIT0;                // P2OUT = pullup
  P2OUT |= BIT3;
  P2DIR |= BIT3;
  P3REN = 0xFF;                 // Tie all P3 ports which are not available on N package
  P3OUT = 0xFF;

  WDTCTL = WDTPW + WDTTMSEL + WDTCNTCL + WDTSSEL; // watchdog counter mode, ACLK, /32768
  IFG1 &= ~WDTIFG;              // Clear WDT interrupt flag
  IE1 |= WDTIE;                 // WDT interrupt enable

  measure_count();              // Establish baseline capacitance
    base_cnt = meas_cnt;

  for(i=15; i>0; i--)           // Repeat and avg base measurement
  {
    measure_count();
      base_cnt = (meas_cnt+base_cnt)/2;
  }


/******************************************************************************/
// Mainloop
/******************************************************************************/

  __enable_interrupt();

  while(1)
  {
      switch(state)
        {
          case NORMAL:
            if (capsense)
            {
              CapTouch();
            }
            if (LEDState==ON)
            {
                HourView();
            }
            if (LEDState==ON && ((hour24>=7) && (hour24<18)))
                {
                  MoonOff();
                  SunOn();
                }
            if (LEDState==ON && ((hour24>=18) || (hour24<7)))
                {
                  SunOff();
                  MoonOn();
                }

            break;
          case SET_HOUR:
            capsense=OFF;
              MoonOff();
              SunOff();
              HourView();
            break;
          case SET_MINUTE:
            capsense=OFF;
              MoonOff();
              SunOff();
              MinuteView();
            break;
          case DEMO:
            if (DEMOPointer==1)
             {
               MoonOn();
               SunOff();
             }
             if (DEMOPointer==7)
             {
               MoonOff();
               SunOn();
             }
             j = 5000/DEMOPointer;
             for(i=0;i<j;i++)
             {
               unsigned int OnTime = 10*DEMOPointer;
               unsigned int OffTime = 120/DEMOPointer;
                 for(LEDPointer = 1;LEDPointer <= DEMOPointer;LEDPointer++)
                 {
                     LEDOn();
                     delay(OnTime);
                     LEDOff();
                     delay(OffTime);
                 }
             }
               DEMOPointer++;
               if (DEMOPointer==13)
               {
               DEMOPointer=1;
               }

            break;

        }
  }

} // main()