Esempio n. 1
0
bool calibration_enter(void)
{
	// If not flying
	if (!sys_state_is_flying())
	{
		calibration_prev_state = sys_get_state();
		calibration_prev_mode = sys_get_mode();
		// Lock vehicle during calibration
		sys_set_mode((uint8_t)MAV_MODE_LOCKED);
		sys_set_state((uint8_t)MAV_STATE_CALIBRATING);
		debug_message_buffer("Starting calibration.");

		mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode,
				global_data.state.status, global_data.cpu_usage, global_data.battery_voltage,
				global_data.motor_block, communication_get_uart_drop_rate());
		debug_message_send_one();
		debug_message_send_one();
		return true;
	}
	else
	{
		//Can't calibrate during flight
		debug_message_buffer("Can't calibrate during flight!!!");
		return false;
	}

}
static void mavlinkStateMachineTask(void* parameters)
{
	// MAVLink protocol state machine
	while (1) {
		// Run handlers, sleep for 20 ms
		// Send setpoints, time out
		// Send one param
		mavlink_pm_queued_send();
		vTaskDelay(5);
		mavlink_wpm_loop();
		vTaskDelay(5);
		// Send one text message
		debug_message_send_one();
		// Wait 20 ms
		vTaskDelay(20);
	}
}
Esempio n. 3
0
void calibration_exit(void)
{
	// Go back to old state
	sys_set_mode(calibration_prev_mode);
	sys_set_state(calibration_prev_state);

	// Clear debug message buffers
	for (int i = 0; i < DEBUG_COUNT; i++)
	{
		debug_message_send_one();
	}

	// Clear UART buffers
	while (uart0_char_available())
	{uart0_get_char();}
	while (uart1_char_available())
	{uart1_get_char();}

	debug_message_buffer("Calibration finished. UART buffers cleared.");
}
Esempio n. 4
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);
		}
	}
}
Esempio n. 5
0
File: main.c Progetto: leitwert/Flow
void send_params_fn(void) {
	debug_message_send_one();
	communication_parameter_send();
}
Esempio n. 6
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
	}
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_ground_car(void)
{
	last_mainloop_idle = sys_time_clock_get_time_usec();
	debug_message_buffer("Starting main loop");
	while (1)
	{
		// Time Measurement
		uint64_t loop_start_time = sys_time_clock_get_time_usec();

		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL 200 Hz functions
		///////////////////////////////////////////////////////////////////////////
		if (us_run_every(5000, COUNTER2, loop_start_time))
		{
			// Kalman Attitude filter, used on all systems
			gyro_read();
			sensors_read_acc();

			// Read out magnetometer at its default 50 Hz rate
			static uint8_t mag_count = 0;
			if (mag_count == 3)
			{
				sensors_read_mag();
				attitude_observer_correct_magnet(global_data.magnet_corrected);
				mag_count = 0;
			}
			else
			{
				mag_count++;
			}

			// Correction step of observer filter
			attitude_observer_correct_accel(global_data.accel_raw);

			// Write in roll and pitch
			static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			attitude_observer_get_angles(&att);
			global_data.attitude.x = att.x;
			global_data.attitude.y = att.y;
			if (global_data.param[PARAM_ATT_KAL_IYAW])
			{
				global_data.attitude.z += 0.005 * global_data.gyros_si.z;
			}
			else
			{
				global_data.attitude.z = att.z;
			}
			// Prediction step of observer
			attitude_observer_predict(global_data.gyros_si);


			// TODO READ OUT MOUSE SENSOR

		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 50 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(20000, COUNTER3, loop_start_time))
		{
			// Read Analog-to-Digital converter
			adc_read();
			// Read remote control
			remote_control();
			// Send the raw sensor/ADC values
			communication_send_raw_data(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// UNCRITICAL SLOW 5 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(200000, COUNTER8, loop_start_time))
		{
			// Send buffered data such as debug text messages
			communication_queued_send();
			// Empty one message out of the buffer
			debug_message_send_one();

			// Toggle status led
			//led_toggle(LED_YELLOW);
			led_toggle(LED_RED); // just for green LED on alpha at the moment

			// Toggle active mode led
			if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
					== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
			{
				led_on(LED_GREEN);
			}
			else
			{
				led_off(LED_GREEN);
			}

			handle_eeprom_write_request();
			handle_reset_request();

			communication_send_remote_control();

			// Pressure sensor driver works, but not tested regarding stability
			sensors_pressure_bmp085_read_out();
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			led_toggle(LED_YELLOW);
			communication_send_attitude_position(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////




		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 200 Hz functions                                     //
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER5, loop_start_time))
		{
			if (global_data.state.status == MAV_STATE_STANDBY)
			{
				//Check if parameters should be written or read
				param_handler();
			}
		}
		///////////////////////////////////////////////////////////////////////////

		else
		{
			// All Tasks are fine and we have no starvation
			last_mainloop_idle = loop_start_time;
		}

		// Read out comm at max rate - takes only a few microseconds in worst case
		communication_receive();

		// MCU load measurement
		uint64_t loop_stop_time = sys_time_clock_get_time_usec();
		global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
		global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);

		if (loop_start_time - last_mainloop_idle >= 5000)
		{
			debug_message_buffer(
					"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
			last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
		}
		if (global_data.cpu_usage > 800)
		{
			// CPU load higher than 80%
			debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
		}
	} // End while(1)

}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_fixed_wing(void)
{
	last_mainloop_idle = sys_time_clock_get_time_usec();
	debug_message_buffer("Starting main loop");
	while (1)
	{
		// Time Measurement
		uint64_t loop_start_time = sys_time_clock_get_time_usec();

		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// Camera Shutter
		///////////////////////////////////////////////////////////////////////////
		// Set camera shutter with 2.5ms resolution
		if (us_run_every(2500, COUNTER1, loop_start_time))
		{
			camera_shutter_handling(loop_start_time);
		}

		if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING)
		{
			///////////////////////////////////////////////////////////////////////////
			/// RC INTERFACE HACK AT 50 Hz
			///////////////////////////////////////////////////////////////////////////
			if (us_run_every(20000, COUNTER8, loop_start_time))
			{
				// Write start byte
				uart1_transmit(0xFF);

				// Write channels 1-6
				for (int i = 1; i < 7; i++)
				{
					uart1_transmit((radio_control_get_channel(1)+1)*127);
				}
			}
			led_toggle(LED_GREEN);
			led_toggle(LED_RED);
			// Do not execute any of the functions below
			continue;
		}

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL 200 Hz functions
		///////////////////////////////////////////////////////////////////////////
		if (us_run_every(5000, COUNTER2, loop_start_time))
		{
			// Kalman Attitude filter, used on all systems
			gyro_read();
			sensors_read_acc();

			// Read out magnetometer at its default 50 Hz rate
			static uint8_t mag_count = 0;
			if (mag_count == 3)
			{
				sensors_read_mag();
				attitude_observer_correct_magnet(global_data.magnet_corrected);
				mag_count = 0;
			}
			else
			{
				mag_count++;
			}

			// Correction step of observer filter
			attitude_observer_correct_accel(global_data.accel_raw);

			// Write in roll and pitch
			static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			attitude_observer_get_angles(&att);
			global_data.attitude.x = att.x;
			global_data.attitude.y = att.y;
			if (global_data.param[PARAM_ATT_KAL_IYAW])
			{
				global_data.attitude.z += 0.005 * global_data.gyros_si.z;
			}
			else
			{
				global_data.attitude.z = att.z;
			}
			// Prediction step of observer
			attitude_observer_predict(global_data.gyros_si);

			control_fixed_wing_attitude();

		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 50 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(20000, COUNTER3, loop_start_time))
		{
			// Read Analog-to-Digital converter
			adc_read();
			// Read remote control
			remote_control();
		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 100 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(10000, COUNTER6, loop_start_time))
		{
			// Send the raw sensor/ADC values
			communication_send_raw_data(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// UNCRITICAL SLOW 5 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(200000, COUNTER8, loop_start_time))
		{
			// The onboard controllers go into failsafe mode once
			// position data is missing
			handle_controller_timeouts(loop_start_time);
			// Send buffered data such as debug text messages
			communication_queued_send();
			// Empty one message out of the buffer
			debug_message_send_one();

			// Toggle status led
			//led_toggle(LED_YELLOW);
			led_toggle(LED_RED); // just for green LED on alpha at the moment

			// Toggle active mode led
			if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
					== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
			{
				led_on(LED_GREEN);
			}
			else
			{
				led_off(LED_GREEN);
			}

			handle_eeprom_write_request();
			handle_reset_request();

			communication_send_controller_feedback();

			communication_send_remote_control();

			// Pressure sensor driver works, but not tested regarding stability
			sensors_pressure_bmp085_read_out();

			if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
			{
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
						90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
						91, global_data.yaw_pos_setpoint);
			}

		}
		///////////////////////////////////////////////////////////////////////////



		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 1 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(1000000, COUNTER9, loop_start_time))
		{
			// Send system state, mode, battery voltage, etc.
			send_system_state();

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//Send GPS information
				float_vect3 gps;
				gps.x = gps_utm_north / 100.0f;//m
				gps.y = gps_utm_east / 100.0f;//m
				gps.z = gps_utm_zone;// gps_week;
				debug_vect("GPS", gps);
			}

			beep_on_low_voltage();

		}
		///////////////////////////////////////////////////////////////////////////




		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			led_toggle(LED_YELLOW);

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//get thru all gps messages
				debug_message_send_one();
			}

			communication_send_attitude_position(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////




		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 200 Hz functions                                     //
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER5, loop_start_time))
		{
			if (global_data.state.status == MAV_STATE_STANDBY)
			{
				//Check if parameters should be written or read
				param_handler();
			}
		}
		///////////////////////////////////////////////////////////////////////////

		else {
			// All Tasks are fine and we have no starvation
			last_mainloop_idle = loop_start_time;
		}

		// Read out comm at max rate - takes only a few microseconds in worst case
		communication_receive();

		// MCU load measurement
		uint64_t loop_stop_time = sys_time_clock_get_time_usec();
		global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
		global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
		if (loop_start_time - last_mainloop_idle >= 5000)
		{
			debug_message_buffer(
					"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
			last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
		}
		if (global_data.cpu_usage > 800)
		{
			// CPU load higher than 80%
			debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
		}
	} // End while(1)

}
void main_loop_quadrotor(void)
{
	/**
	 * @brief Initialize the whole system
	 *
	 * All functions that need to be called before the first mainloop iteration
	 * should be placed here.
	 */
	main_init_generic();
	control_quadrotor_position_init();
	control_quadrotor_attitude_init();
	attitude_tobi_laurens_init();

	// FIXME XXX Make proper mode switching

//	outdoor_position_kalman_init();
	//vision_position_kalman_init();

	// Default filters, allow Vision, Vicon and optical flow inputs
	vicon_position_kalman_init();
	optflow_speed_kalman_init();

	/**
	 * @brief This is the main loop
	 *
	 * It will be executed at maximum MCU speed (60 Mhz)
	 */
	// Executiontime debugging
	time_debug.x = 0;
	time_debug.y = 0;
	time_debug.z = 0;

	last_mainloop_idle = sys_time_clock_get_time_usec();
	debug_message_buffer("Starting main loop");

	led_off(LED_GREEN);
	led_off(LED_RED);
	while (1)
	{
		// Time Measurement
		uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL 200 Hz functions
		///////////////////////////////////////////////////////////////////////////
		if (us_run_every(5000, COUNTER2, loop_start_time))
		{
			// Kalman Attitude filter, used on all systems
			gyro_read();
			sensors_read_acc();

			sensors_pressure_bmp085_read_out();

			// Read out magnetometer at its default 50 Hz rate
			static uint8_t mag_count = 0;

			if (mag_count == 3)
			{
				sensors_read_mag();
				//attitude_observer_correct_magnet(global_data.magnet_corrected);
				mag_count = 0;
			}else if(mag_count==1){

				hmc5843_start_read();
				mag_count++;
			}
			else
			{
				mag_count++;
			}

			// Correction step of observer filter
			attitude_tobi_laurens();

			if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY ||
				global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP)
			{
				vicon_position_kalman();
			}
			else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY)
			{
				outdoor_position_kalman();
			}

			control_quadrotor_attitude();

			//debug counting number of executions
			count++;
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// Camera Shutter - This takes 50 usecs!!!
		///////////////////////////////////////////////////////////////////////////
		// Set camera shutter with 2.5ms resolution
		else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!!
		{
			camera_shutter_handling(loop_start_time);

			// Measure time for debugging
			time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec()
					- loop_start_time);

		}

		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 50 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(20000, COUNTER3, loop_start_time))
		{
			// Read infrared sensor
			//adc_read();

			// Control the quadrotor position
			control_quadrotor_position();
			// Read remote control
			remote_control();

			control_camera_angle();

//			//float_vect3 opt;
//			static float_vect3 opt_int;
//			uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z);
//			if (valid)
//			{
//				opt_int.x += global_data.optflow.x;
//				opt_int.y += global_data.optflow.y;
//
//			}
//
//			uint8_t supersampling = 10;
//			for (int i = 0; i < supersampling; ++i)
//			{
//				global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL);
//			}
//
//			global_data.sonar_distance /= supersampling;
//
//			opt_int.z = valid;
//			static unsigned int i = 0;
//			if (i == 10)
//			{
//				mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered);
//
//				i = 0;
//			}
//			i++;
			//optical_flow_debug_vect_send();
			//debug_vect("opt_int", opt_int);
//			optical_flow_start_read(80);

			if (global_data.state.position_estimation_mode
					== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION
					|| global_data.state.position_estimation_mode
							== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION)
			{
				optflow_speed_kalman();
			}

			// Send the raw sensor/ADC values
			communication_send_raw_data(loop_start_time);

			float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f;
			debug_vect("yaw_low", yy);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// CRITICAL FAST 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER4, loop_start_time))
		{
			//*** this happens in handle_controller_timeouts already!!!!! ***
			//			//update global_data.state
			//			if (global_data.param[PARAM_VICON_MODE] == 1)
			//			{
			//				//VICON_MODE 1 only accepts vicon position
			//				global_data.state.position_fix = global_data.state.vicon_ok;
			//			}
			//			else
			//			{
			//				//VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision
			//				global_data.state.position_fix = global_data.state.vision_ok;
			//			}

			update_system_statemachine(loop_start_time);
			update_controller_setpoints();

			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(
					global_data.param[PARAM_SEND_DEBUGCHAN],
					sys_time_clock_get_loop_start_time_boot_ms(),
					global_data.attitude_setpoint.x,
					global_data.attitude_setpoint.y,
					global_data.position_yaw_control_output,
					global_data.thrust_control_output);

			//STARTING AND LANDING
			quadrotor_start_land_handler(loop_start_time);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 100 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER6, loop_start_time))
		{

			if (global_data.param[PARAM_SEND_SLOT_DEBUG_6])
			{
				debug_vect("att_ctrl_o", global_data.attitude_control_output);
			}
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// UNCRITICAL SLOW 5 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(200000, COUNTER8, loop_start_time))
		{
			// The onboard controllers go into failsafe mode once
			// position data is missing
			handle_controller_timeouts(loop_start_time);
			// Send buffered data such as debug text messages
			// Empty one message out of the buffer
			debug_message_send_one();

			// Toggle status led
			led_toggle(LED_RED);

			// Toggle active mode led
			if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
			{
				led_on(LED_GREEN);
			}
			else
			{
				led_off(LED_GREEN);
			}

			handle_eeprom_write_request();
			handle_reset_request();

			update_controller_parameters();

			communication_send_controller_feedback();

			communication_send_remote_control();

			// Pressure sensor driver works, but not tested regarding stability
			//			sensors_pressure_bmp085_read_out();

			if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
			{
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
						90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
				mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
						91, global_data.yaw_pos_setpoint);
			}
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 1 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(1000000, COUNTER9, loop_start_time))
		{
			// Send system state, mode, battery voltage, etc.
			send_system_state();

			// Send position setpoint offset
			//debug_vect("pos offs", global_data.position_setpoint_offset);

			// Send current onboard time
			mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
			mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());

			//update state from received parameters
			sync_state_parameters();

			//debug number of execution
			count = 0;

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//Send GPS information
				float_vect3 gps;
				gps.x = gps_utm_north / 100.0f;//m
				gps.y = gps_utm_east / 100.0f;//m
				gps.z = gps_utm_zone;// gps_week;
				debug_vect("GPS", gps);

			}
			else if (global_data.param[PARAM_GPS_MODE] == 9
					|| global_data.param[PARAM_GPS_MODE] == 8)
			{

				if (global_data.param[PARAM_GPS_MODE] == 8)
				{
					gps_set_local_origin();
					//					gps_local_home_init = false;
				}
				if (gps_lat == 0)
				{
					debug_message_buffer("GPS Signal Lost");
				}
				else
				{
					float_vect3 gps_local, gps_local_velocity;
					gps_get_local_position(&gps_local);
					debug_vect("GPS local", gps_local);
					gps_get_local_velocity(&gps_local_velocity);
					debug_vect("GPS loc velocity", gps_local_velocity);
				}
			}
			if (global_data.state.gps_mode)
			{
				gps_send_local_origin();
			}
			beep_on_low_voltage();

		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 20 Hz functions
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(50000, COUNTER7, loop_start_time))
		{
			//led_toggle(LED_YELLOW);

			if (global_data.param[PARAM_GPS_MODE] >= 10)
			{
				//get thru all gps messages
				debug_message_send_one();
			}

			communication_send_attitude_position(loop_start_time);

			// Send parameter
			communication_queued_send();

//			//infrared distance
//			float_vect3 infra;
//			infra.x = global_data.ground_distance;
//			infra.y = global_data.ground_distance_unfiltered;
//			infra.z = global_data.state.ground_distance_ok;
//			debug_vect("infrared", infra);
		}
		///////////////////////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////////////////////
		/// NON-CRITICAL SLOW 200 Hz functions                                     //
		///////////////////////////////////////////////////////////////////////////
		else if (us_run_every(5000, COUNTER5, loop_start_time))
		{
			if (global_data.state.status == MAV_STATE_STANDBY)
			{
				//Check if parameters should be written or read
				param_handler();
			}
		}
		///////////////////////////////////////////////////////////////////////////

		else
		{
			// All Tasks are fine and we have no starvation
			last_mainloop_idle = loop_start_time;
		}

		// Read out comm at max rate - takes only a few microseconds in worst case
		communication_receive();

		// MCU load measurement
		uint64_t loop_stop_time = sys_time_clock_get_time_usec();
		global_data.cpu_usage = measure_avg_cpu_load(loop_start_time,
				loop_stop_time, min_mainloop_time);
		global_data.cpu_peak = measure_peak_cpu_load(loop_start_time,
				loop_stop_time, min_mainloop_time);
		time_debug.y = max(time_debug.y, global_data.cpu_usage);
		time_debug.z = max(time_debug.z, global_data.cpu_peak);
		if (loop_start_time - last_mainloop_idle >= 20000)
		{
			debug_message_buffer(
					"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
			last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
		}
		if (global_data.cpu_usage > 800)
		{
			// CPU load higher than 80%
			debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
		}
	} // End while(1)

}
Esempio n. 10
0
File: main.c Progetto: 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);
		}
	}
}