Ejemplo n.º 1
0
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Ejemplo n.º 2
0
static inline void mavlink_send_local_position_ned(void)
{
  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
                                      get_sys_time_msec(),
                                      stateGetPositionNed_f()->x,
                                      stateGetPositionNed_f()->y,
                                      stateGetPositionNed_f()->z,
                                      stateGetSpeedNed_f()->x,
                                      stateGetSpeedNed_f()->y,
                                      stateGetSpeedNed_f()->z);
  MAVLinkSendMessage();
}
Ejemplo n.º 3
0
	void send(const hrt_abstime t)
	{
		if (pos_sub->update(t)) {
			mavlink_msg_local_position_ned_send(_channel,
							    pos->timestamp / 1000,
							    pos->x,
							    pos->y,
							    pos->z,
							    pos->vx,
							    pos->vy,
							    pos->vz);
		}
	}
Ejemplo n.º 4
0
	void send(const hrt_abstime t)
	{
		struct vehicle_local_position_s pos;

		if (pos_sub->update(&pos_time, &pos)) {
			mavlink_msg_local_position_ned_send(_channel,
							    pos.timestamp / 1000,
							    pos.x,
							    pos.y,
							    pos.z,
							    pos.vx,
							    pos.vy,
							    pos.vz);
		}
	}
Ejemplo n.º 5
0
void
l_local_position(const struct listener *l)
{
	/* copy local position data into local buffer */
	orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);

	if (gcs_link)
		mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
						    local_pos.timestamp / 1000,
						    local_pos.x,
						    local_pos.y,
						    local_pos.z,
						    local_pos.vx,
						    local_pos.vy,
						    local_pos.vz);
}
Ejemplo n.º 6
0
/*
  send LOCAL_POSITION_NED message
 */
void GCS_MAVLINK::send_local_position(const AP_AHRS &ahrs) const
{
    Vector3f local_position, velocity;
    if (!ahrs.get_relative_position_NED(local_position) ||
        !ahrs.get_velocity_NED(velocity)) {
        // we don't know the position and velocity
        return;
    }

    mavlink_msg_local_position_ned_send(
        chan,
        hal.scheduler->millis(),
        local_position.x,
        local_position.y,
        local_position.z,
        velocity.x,
        velocity.y,
        velocity.z);
}
Ejemplo n.º 7
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
	}
}
Ejemplo n.º 8
0
Archivo: main.c Proyecto: 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);
		}
	}
}