/**
* @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);
}
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);
}
/**
@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;
}
}
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;
}
}
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);
}
}
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;
}
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);
}
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);
}
}
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);
}
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);
}
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;
}
void AllLEDOn() {
uint8_t i;
for (i = 0; i < 8; i++)
LEDOn(i);
}
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();
}
}
}
/**
* @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(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_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(¤t_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);
}
}
}
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);
}
}
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);
}
/**
* @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(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_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(¤t_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);
}
}
}
/**
* @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(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_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(¤t_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
}
}
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()
