Exemplo n.º 1
0
int16_t main(void) {
	//initialize all system clocks
    init_clock();
	//initialize serial communications
    init_uart();
	//initialize pin driving library (to be able to use the &D[x] defs)
	init_pin();
	//initialize the UI library
    init_ui();
	//initialize the timer module
    init_timer();
	//initialize the OC module (used by the servo driving code)
	init_oc();
	
    imu_init()
	//Set servo control pins as output
	pin_digitalOut(PAN_PIN);
	pin_digitalOut(TILT_PIN);
	pin_digitalOut(SONIC_OUT_PIN);
	pin_digitalIn(SONIC_IN_PIN);
	
	//Set LED off
	led_off(LED);
	//Configure blinking rate for LED when connected
    timer_setPeriod(LED_TIM, 0.2);
    timer_start(LED_TIM);
	

    //Configure timer for reciever timeout
    timer_setPeriod(DIST_TIM, 0.05);


	//configure PWM on sonic output pin
	oc_pwm(PWM_OC, SONIC_OUT_PIN, NULL, SONIC_FREQ, 0x0000);
	
	//According to HobbyKing documentation: range .8 through 2.2 msec
	//Set servo control pins as OC outputs on their respective timers
	oc_servo(SERVO1_OC, PAN_PIN, 	SERVO1_TIM, SERVO_PERIOD, SERVO_MIN, SERVO_MAX, pan_set_val);
	oc_servo(SERVO2_OC, TILT_PIN, 	SERVO2_TIM, SERVO_PERIOD, SERVO_MIN, SERVO_MAX, tilt_set_val);

    InitUSB();                              // initialize the USB registers and serial interface engine
    while (USB_USWSTAT!=CONFIG_STATE) {     // while the peripheral is not configured...
        ServiceUSB();                       // ...service USB requests
		led_on(LED);
		//There's no point in driving the servos when there's no one connected yet.
    }
	
    while (1) {
        ServiceUSB();                       // service any pending USB requests
		
		//blink the LED
		if (timer_flag(LED_TIM)) {
            timer_lower(LED_TIM);
            led_toggle(LED);
        }
		
		//Update the servo control values.
        x_gout = gyro_read(OUT_X_L);
    }
}
Exemplo n.º 2
0
void gyro_set_measure_mode(void) {
	unsigned char temp;
	//get out of powerdown CTRL_REG1 bit3 = 1
	temp = gyro_read(CTRL_REG1);
	temp |= 0xF8;
	gyro_write(CTRL_REG1, temp);
}
Exemplo n.º 3
0
void FreeIMURaw(void)
{
	char outbuff[60];
	Point3df accel_xyz, gyro_xyz, mag_xyz;

	accelerometer_read(&accel_xyz);
	gyro_read(&gyro_xyz);
	MagPointRaw(&mag_xyz);

	sprintf(outbuff, "%d,%d,%d,%d,%d,%d,%d,%d,%d,0,0,\n",(int)accel_xyz.x, (int)accel_xyz.y, (int)accel_xyz.z, (int)gyro_xyz.x, (int)gyro_xyz.y, (int)gyro_xyz.z, (int)mag_xyz.x, (int)mag_xyz.y, (int)mag_xyz.z);
	VCP_write(outbuff, strlen(outbuff));
}
Exemplo 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);
		}
	}
}
Exemplo n.º 5
0
void flightControl(void) {



  // this should run at 500Hz
  if ((micros() - elapsed_micros) >= (2000)) {
    elapsed_micros = micros();

    // flicker the leds to measure correct speed
    GPIOA->ODR ^= 1<<15;
    GPIOB->ODR ^= 1<<2;

    // read gyro
    gyro_read(&gyro_data);

    // controls
/*
    throttle   = payload[0] * 0xff;
    yaw        = payload[1] * 0xff;
    pitch      = payload[3] * 0xff;
    roll       = payload[4] * 0xff;

    // trims
    yaw_trim   = payload[2];
    pitch_trim = payload[5];
    roll_trim  = payload[6];
*/


    // convert channels for crazyflie PID:
#if 0
    rollRateDesired = (float) (payload[4] - 128.0) * 256;
    pitchRateDesired = (float) (payload[3] - 128.0) * 256;
    yawRateDesired = (float) (payload[1] - 128.0) * 256;

#else
    actuatorThrust = payload[0] * 256;
    // Roll, aka aileron, float +- 50.0 in degrees
    s32 f_roll = (payload[4] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
    frac2float(f_roll, &rollRateDesired);

    // Pitch, aka elevator, float +- 50.0 degrees
    s32 f_pitch = (payload[3] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
    frac2float(f_pitch, &pitchRateDesired);

    // Thrust, aka throttle 0..65535, working range 5535..65535
    // No space for overshoot here, hard limit Channel3 by -10000..10000
/*
    s32 ch = payload[0] * 0xff;
    if (ch < CHAN_MIN_VALUE) {
        ch = CHAN_MIN_VALUE;
    } else if (ch > CHAN_MAX_VALUE) {
        ch = CHAN_MAX_VALUE;
    }
    actuatorThrust  = ch*3L + 35535L;
*/

    // Yaw, aka rudder, float +- 400.0 deg/s
    s32 f_yaw = (payload[1] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
    frac2float(f_yaw, &yawRateDesired);
#endif

    // gyro.* == *rateActual == Data measured by IMU
    // *rateDesired == Data from RX
    controllerCorrectRatePID(-gyro_data.x, gyro_data.y, -gyro_data.z, rollRateDesired, pitchRateDesired, yawRateDesired);

//#define TUNE_ROLL

    if (actuatorThrust > 0)
    {
#if defined(TUNE_ROLL)
      distributePower(actuatorThrust, rollOutput, 0, 0);
#elif defined(TUNE_PITCH)
      distributePower(actuatorThrust, 0, pitchOutput, 0);
#elif defined(TUNE_YAW)
      distributePower(actuatorThrust, 0, 0, -yawOutput);
#else
      distributePower(actuatorThrust, rollOutput, pitchOutput, -yawOutput);
#endif
    }
    else
    {
      distributePower(0, 0, 0, 0);
      pidReset(&pidRollRate);
      pidReset(&pidPitchRate);
      pidReset(&pidYawRate);
    }


  }
#if 0
    m0_val = actuatorThrust;
    m1_val = actuatorThrust;
    m2_val = actuatorThrust;
    m3_val = ((yawOutput * 1000) / 0xffff) + 1000;
    TIM2->CCR4  = m0_val; // Motor "B"
    TIM1->CCR1  = m1_val; // Motor "L"
    TIM1->CCR4  = m2_val; // Motor "R"
    TIM16->CCR1 = m3_val; // Motor "F"
#endif
}
Exemplo n.º 6
0
static int
gyro_tick_do(struct gyroscope_l3g4200d_data *mdata)
{
    struct sol_direction_vector val =
    {
        .min = -GYRO_RANGE,
        .max = GYRO_RANGE,
        .x = mdata->reading[0],
        .y = mdata->reading[1],
        .z = mdata->reading[2]
    };
    int r;

    gyro_read(mdata);

    r = sol_flow_send_direction_vector_packet
            (mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT,
            &val);

    return r;
}

static bool
gyro_ready(void *data)
{
    struct gyroscope_l3g4200d_data *mdata = data;

    mdata->timer = NULL;
    mdata->ready = true;

    while (mdata->pending_ticks) {
        gyro_tick_do(mdata);
        mdata->pending_ticks--;
    }

    SOL_DBG("gyro is ready for reading");

    return false;
}

static bool
gyro_init_stream(void *data)
{
    bool r;
    struct gyroscope_l3g4200d_data *mdata = data;
    static const uint8_t value = GYRO_REG_FIFO_CTL_STREAM;

    if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
        SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
        return false;
    }

    /* enable FIFO in stream mode */
    r = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, &value, 1);
    if (!r) {
        SOL_WRN("could not set L3G4200D gyro sensor's stream mode");
        return false;
    }

    if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready, mdata) < 0)
        SOL_WRN("error in scheduling a L3G4200D gyro's init command");

    return false;
}
/**
 * \brief Returns a reading from the sensor
 * \param type MPU_9250_SENSOR_TYPE_ACC_[XYZ] or MPU_9250_SENSOR_TYPE_GYRO_[XYZ]
 * \return centi-G (ACC) or centi-Deg/Sec (Gyro)
 */
static int
value(int type)
{
  int rv;
  float converted_val = 0;

  if(state == SENSOR_STATE_DISABLED) {
    PRINTF("MPU: Sensor Disabled\n");
    return CC26XX_SENSOR_READING_ERROR;
  }

  memset(sensor_value, 0, sizeof(sensor_value));

  if((type & MPU_9250_SENSOR_TYPE_ACC) != 0) {
    t0 = RTIMER_NOW();

    while(!int_status() &&
          (RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));

    rv = acc_read(sensor_value);

    if(rv == 0) {
      return CC26XX_SENSOR_READING_ERROR;
    }

    PRINTF("MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
           sensor_value[0], sensor_value[1], sensor_value[2]);

    /* Convert */
    if(type == MPU_9250_SENSOR_TYPE_ACC_X) {
      converted_val = acc_convert(sensor_value[0]);
    } else if(type == MPU_9250_SENSOR_TYPE_ACC_Y) {
      converted_val = acc_convert(sensor_value[1]);
    } else if(type == MPU_9250_SENSOR_TYPE_ACC_Z) {
      converted_val = acc_convert(sensor_value[2]);
    }
    rv = (int)(converted_val * 100);
  } else if((type & MPU_9250_SENSOR_TYPE_GYRO) != 0) {
    t0 = RTIMER_NOW();

    while(!int_status() &&
          (RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));

    rv = gyro_read(sensor_value);

    if(rv == 0) {
      return CC26XX_SENSOR_READING_ERROR;
    }

    PRINTF("MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
           sensor_value[0], sensor_value[1], sensor_value[2]);

    if(type == MPU_9250_SENSOR_TYPE_GYRO_X) {
      converted_val = gyro_convert(sensor_value[0]);
    } else if(type == MPU_9250_SENSOR_TYPE_GYRO_Y) {
      converted_val = gyro_convert(sensor_value[1]);
    } else if(type == MPU_9250_SENSOR_TYPE_GYRO_Z) {
      converted_val = gyro_convert(sensor_value[2]);
    }
    rv = (int)(converted_val * 100);
  } else {
    PRINTF("MPU: Invalid type\n");
    rv = CC26XX_SENSOR_READING_ERROR;
  }

  PRINTF("%ld\n", (long int)(converted_val * 100));

  return rv;
}
Exemplo n.º 8
0
Arquivo: main.c Projeto: leitwert/Flow
/**
  * @brief  Main function.
  */
int main(void)
{
	__enable_irq();
	snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem);

	/* 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 timers */
	timer_init();

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

	/* init mavlink */
	communication_init();

	/* initialize camera: */
	img_stream_param.size.x = FLOW_IMAGE_SIZE;
	img_stream_param.size.y = FLOW_IMAGE_SIZE;
	img_stream_param.binning = 4;
	{
		camera_image_buffer buffers[5] = {
			BuildCameraImageBuffer(image_buffer_8bit_1),
			BuildCameraImageBuffer(image_buffer_8bit_2),
			BuildCameraImageBuffer(image_buffer_8bit_3),
			BuildCameraImageBuffer(image_buffer_8bit_4),
			BuildCameraImageBuffer(image_buffer_8bit_5)
		};
		camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(), 
					mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0,
					&img_stream_param, buffers, 5);
	}

	/* gyro config */
	gyro_config();

	/* 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_register(sonar_update_fn, SONAR_POLL_MS);
	timer_register(system_state_send_fn, SYSTEM_STATE_MS);
	timer_register(system_receive_fn, SYSTEM_STATE_MS / 2);
	timer_register(send_params_fn, PARAMS_MS);
	timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]);
	timer_register(take_snapshot_fn, 500);
	//timer_register(switch_params_fn, 2000);

	/* variables */
	uint32_t counter = 0;

	result_accumulator_ctx mavlink_accumulator;

	result_accumulator_init(&mavlink_accumulator);
	
	uint32_t fps_timing_start = get_boot_time_us();
	uint16_t fps_counter = 0;
	uint16_t fps_skipped_counter = 0;
	
	uint32_t last_frame_index = 0;
	
	/* main loop */
	while (1)
	{
		/* check timers */
		timer_check();
		
		if (snap_capture_done) {
			snap_capture_done = false;
			camera_snapshot_acknowledge(&cam_ctx);
			snap_ready = true;
			if (snap_capture_success) {
				/* send the snapshot! */
				LEDToggle(LED_COM);
				mavlink_send_image(&snapshot_buffer);
			}
		}

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

		/* 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 to flow sensor coordinates */
		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

		/* 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;
		}

		bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0;

		uint32_t start_computations = 0;
		
		/* get recent images */
		camera_image_buffer *frames[2];
		camera_img_stream_get_buffers(&cam_ctx, frames, 2, true);
		
		start_computations = get_boot_time_us();
		
		int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index);
		last_frame_index = frames[0]->frame_number;
		fps_skipped_counter += frame_delta - 1;
		
		flow_klt_image *klt_images[2] = {NULL, NULL};
		{
			/* make sure that the new images get the correct treatment */
			/* this algorithm will still work if both images are new */
			int i;
			bool used_klt_image[2] = {false, false};
			for (i = 0; i < 2; ++i) {
				if (frames[i]->frame_number != frames[i]->meta) {
					// the image is new. apply pre-processing:
					/* filter the new image */
					if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) {
						filter_image(frames[i]->buffer, frames[i]->param.p.size.x);
					}
					/* update meta data to mark it as an up-to date image: */
					frames[i]->meta = frames[i]->frame_number;
				} else {
					// the image has the preprocessing already applied.
					if (use_klt) {
						int j;
						/* find the klt image that matches: */
						for (j = 0; j < 2; ++j) {
							if (flow_klt_images[j].meta == frames[i]->frame_number) {
								used_klt_image[j] = true;
								klt_images[i] = &flow_klt_images[j];
							}
						}
					}
				}
			}
			if (use_klt) {
				/* only for KLT: */
				/* preprocess the images if they are not yet preprocessed */
				for (i = 0; i < 2; ++i) {
					if (klt_images[i] == NULL) {
						// need processing. find unused KLT image:
						int j;
						for (j = 0; j < 2; ++j) {
							if (!used_klt_image[j]) {
								used_klt_image[j] = true;
								klt_images[i] = &flow_klt_images[j];
								break;
							}
						}
						klt_preprocess_image(frames[i]->buffer, klt_images[i]);
					}
				}
			}
		}
		
		float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f;

		/* compute gyro rate in pixels and change to image coordinates */
		float x_rate_px = - y_rate * (focal_length_px * frame_dt);
		float y_rate_px =   x_rate * (focal_length_px * frame_dt);
		float z_rate_fr = - z_rate * frame_dt;

		/* compute optical flow in pixels */
		flow_raw_result flow_rslt[32];
		uint16_t flow_rslt_count = 0;
		if (!use_klt) {
			flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
		} else {
			flow_rslt_count =  compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
		}

		/* calculate flow value from the raw results */
		float pixel_flow_x;
		float pixel_flow_y;
		float outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO];
		float min_outlier_threshold = 0;
		if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0)
		{
			min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK];
		}else
		{
			min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT];
		}
		uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y, 
							outlier_threshold,  min_outlier_threshold);

		/* create flow image if needed (previous_image is not needed anymore)
		 * -> can be used for debugging purpose
		 */
		previous_image = frames[1];
		if (global_data.param[PARAM_USB_SEND_VIDEO])
		{
			uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH];
			uint8_t *prev_img = previous_image->buffer;
			for (int i = 0; i < flow_rslt_count; i++) {
				if (flow_rslt[i].quality > 0) {
					prev_img[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255;
					int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f);
					if (ofs >= 0 && ofs < frame_size * frame_size) {
						prev_img[ofs] = 200;
					}
				}
			}
		}

		/* return the image buffers */
		camera_img_stream_return_buffers(&cam_ctx, frames, 2);
		
		/* decide which distance to use */
		float ground_distance = 0.0f;

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

		/* update I2C transmit buffer */
		update_TX_buffer(frame_dt, 
						 x_rate, y_rate, z_rate, gyro_temp, 
						 qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, 
						 distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));

		/* accumulate the results */
		result_accumulator_feed(&mavlink_accumulator, frame_dt, 
								x_rate, y_rate, z_rate, gyro_temp, 
								qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px, 
								distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));

		uint32_t computaiton_time_us = get_time_delta_us(start_computations);

		counter++;
		fps_counter++;

        /* serial mavlink  + usb mavlink output throttled */
		if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
		{
			float fps = 0;
			float fps_skip = 0;
			if (fps_counter + fps_skipped_counter > 100) {
				uint32_t dt = get_time_delta_us(fps_timing_start);
				fps_timing_start += dt;
				fps = (float)fps_counter / ((float)dt * 1e-6f);
				fps_skip = (float)fps_skipped_counter / ((float)dt * 1e-6f);
				fps_counter = 0;
				fps_skipped_counter = 0;

				mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip);
			}
			mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(), 
					frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness);
			
			/* calculate the output values */
			result_accumulator_output_flow output_flow;
			result_accumulator_output_flow_rad output_flow_rad;
			int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO];
			result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow);
			result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad);

			// send flow
			mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
					output_flow.flow_x, output_flow.flow_y,
					output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, 
					output_flow.quality, output_flow.ground_distance);

			mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
					output_flow_rad.integration_time, 
					output_flow_rad.integrated_x, output_flow_rad.integrated_y,
					output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
					output_flow_rad.temperature, output_flow_rad.quality,
					output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance);

			if (global_data.param[PARAM_USB_SEND_FLOW] && (output_flow.quality > 0 || global_data.param[PARAM_USB_SEND_QUAL_0]))
			{
				mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
						output_flow.flow_x, output_flow.flow_y,
						output_flow.flow_comp_m_x, output_flow.flow_comp_m_y, 
						output_flow.quality, output_flow.ground_distance);

				mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
						output_flow_rad.integration_time, 
						output_flow_rad.integrated_x, output_flow_rad.integrated_y,
						output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
						output_flow_rad.temperature, output_flow_rad.quality,
						output_flow_rad.time_delta_distance_us,output_flow_rad.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);
			}

			result_accumulator_reset(&mavlink_accumulator);
		}

		/* forward flow from other sensors */
		if (counter % 2)
		{
			communication_receive_forward();
		}
	}
}
Exemplo n.º 9
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
	}
}
Exemplo n.º 10
0
/**
* @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)

}
Exemplo n.º 11
0
/**
* @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)

}
Exemplo n.º 12
0
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)

}
Exemplo n.º 13
0
int main(void)
{

	clk_init();
	
	delay(1000);
	
  gpio_init();

#ifdef SERIAL	
	serial_init();
#endif

	i2c_init();
	
	spi_init();
	
	pwm_init();

	pwm_set( MOTOR_FL , 0);   // FL
	pwm_set( MOTOR_FR , 0);	 
	pwm_set( MOTOR_BL , 0);   // BL
	pwm_set( MOTOR_BR , 0);   // FR

  time_init();
	

#ifdef SERIAL	
	printf( "\n clock source:" );
	#endif
	if (  RCC_GetCK_SYSSource() == 8)  
	{
		#ifdef SERIAL	
		printf( " PLL \n" );
		#endif
	}
	else 
	{
	#ifdef SERIAL	
	if (  RCC_GetCK_SYSSource() == 0)  printf( " HSI \n" );
	else  printf( " OTHER \n" );
	#endif
  failloop( 5 );
	}

	sixaxis_init();
	
	if ( sixaxis_check() ) 
	{
		#ifdef SERIAL	
		printf( " MPU found \n" );
		#endif
	}
	else 
	{
		#ifdef SERIAL	
		printf( "ERROR: MPU NOT FOUND \n" );	
		#endif
		failloop(4);
	}
	
	adc_init();
	
	rx_init();
	
int count = 0;
	
while ( count < 64 )
{
	vbattfilt += adc_read(1);
	count++;
}
 vbattfilt = vbattfilt/64;	

#ifdef SERIAL	
		printf( "Vbatt %2.2f \n", vbattfilt );
		#ifdef NOMOTORS
    printf( "NO MOTORS\n" );
		#warning "NO MOTORS"
		#endif
#endif
	
#ifdef STOP_LOWBATTERY
// infinite loop
if ( vbattfilt < (float) STOP_LOWBATTERY_TRESH) failloop(2);
#endif

	gyro_cal();

	
extern unsigned int liberror;
if ( liberror ) 
{
	  #ifdef SERIAL	
		printf( "ERROR: I2C \n" );	
		#endif
		failloop(7);
}


 static unsigned lastlooptime; 
 lastlooptime = gettime();
 extern int rxmode;
 extern int failsafe;

 float thrfilt;

//
//
// 		MAIN LOOP
//
//

#ifdef DEBUG
static float timefilt;
#endif

	while(1)
	{
		// gettime() needs to be called at least once per second 
		unsigned long time = gettime(); 
		looptime = ((uint32_t)( time - lastlooptime));
		if ( looptime <= 0 ) looptime = 1;
		looptime = looptime * 1e-6f;
		if ( looptime > 0.02f ) // max loop 20ms
		{
			failloop( 3);	
			//endless loop			
		}
	
		#ifdef DEBUG				
		totaltime += looptime;
		lpf ( &timefilt , looptime, 0.998 );
		#endif
		lastlooptime = time;
		
		if ( liberror > 20) 
		{
			failloop(8);
			// endless loop
		}
		
		checkrx();
		
		gyro_read();
		
		control();
		
// battery low logic
		static int lowbatt = 0;
		float hyst;
		float battadc = adc_read(1);

		// average of all 4 motor thrusts
		// should be proportional with battery current			
		extern float thrsum; // from control.c
		// filter motorpwm so it has the same delay as the filtered voltage
		// ( or they can use a single filter)		
		lpf ( &thrfilt , thrsum , 0.9968f);	// 0.5 sec at 1.6ms loop time	
		
		lpf ( &vbattfilt , battadc , 0.9968f);		

		if ( lowbatt ) hyst = HYST;
		else hyst = 0.0f;
		
		if ( vbattfilt + (float) VDROP_FACTOR * thrfilt <(float) VBATTLOW + hyst ) lowbatt = 1;
		else lowbatt = 0;
		
// led flash logic		
		if ( rxmode == 0)
		{// bind mode
		ledflash ( 100000+ 500000*(lowbatt) , 12);
		}else
		{// non bind
		if ( failsafe) 
		{
		if ( lowbatt )
				ledflash ( 500000 , 8);
		else
				ledflash ( 500000, 15);	
			
		}
			else
			{					
			if ( lowbatt) 
				 ledflash ( 500000 , 8);	
			else ledon( 255);	
			} 		
		}

// the delay is required or it becomes endless loop ( truncation in time routine)
while ( (gettime() - time) < 1000 ) delay(10); 		

		
	}// end loop
	

}
Exemplo n.º 14
0
void FreeIMUSendYawPitchRoll(int count)
{
	Point3df gyro_xyz;
	Point3df accel_xyz;
	Point3df mag_xyz;
	char outbuff[60];
	int pos=0, i;

	if(filter_init == 0){
		memset(&gyro_xyz_filtered, 0, sizeof(Point3df));
		filter_init = 1;
	}

	ExpLedOn(ORANGE_LED);

	for(i=0; i<count; i++){
		ExpLedToggle(ORANGE_LED);

		gyro_read(&gyro_xyz);
		accelerometer_read(&accel_xyz);
		MagPointRaw(&mag_xyz);

		// Apply calibration values
		accel_xyz.x = (accel_xyz.x - (float)calibration.offsets[0]) / calibration.scales[0];
		accel_xyz.y = (accel_xyz.y - (float)calibration.offsets[1]) / calibration.scales[1];
		accel_xyz.z = (accel_xyz.z - (float)calibration.offsets[2]) / calibration.scales[2];

		mag_xyz.x = (mag_xyz.x - (float)calibration.offsets[3]) / calibration.scales[3];
		mag_xyz.y = (mag_xyz.y - (float)calibration.offsets[4]) / calibration.scales[4];
		mag_xyz.z = (mag_xyz.z - (float)calibration.offsets[5]) / calibration.scales[5];

		// Normalize acceleration measurements so they range from 0 to 1
		float accxnorm = accel_xyz.x/sqrtf(accel_xyz.x*accel_xyz.x+accel_xyz.y*accel_xyz.y+accel_xyz.z*accel_xyz.z);
		float accynorm = accel_xyz.y/sqrtf(accel_xyz.x*accel_xyz.x+accel_xyz.y*accel_xyz.y+accel_xyz.z*accel_xyz.z);

		// calculate pitch and roll
		float Pitch = asinf(-accxnorm);
		float Roll = asinf(accynorm/cosf(Pitch));

		// tilt compensated magnetic sensor measurements
		float magxcomp = mag_xyz.x*cosf(Pitch)+mag_xyz.z*sinf(Pitch);
		float magycomp = mag_xyz.x*sinf(Roll)*sinf(Pitch)+mag_xyz.y*cosf(Roll)-mag_xyz.z*sinf(Roll)*cosf(Pitch);

		// arctangent of y/x converted to degrees
		float heading = (float)(180*atan2f(magycomp,magxcomp)/PI);
		if(heading < 0)
		heading +=360;
		previous_time = (float)HAL_GetTick();

/*
		float tau=0.075;
		float a=0.0;
		float Complementary(float newAngle, float newRate,int looptime) {
		float dtC = float(looptime)/1000.0;
		a=tau/(tau+dtC);
		x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
		return x_angleC;
		}
*/
/*
		// Adjust the gyro readings with a complimentary filter
		// angle = 0.98 *(angle+gyro*dt) + 0.02*acc
		if(previous_time > 0){
		float dt = ((float)HAL_GetTick() - previous_time)/1000000;
		gyro_xyz.x = 0.98 *(gyro_xyz.x+gyro_xyz.x*dt) + 0.02*accel_xyz.x;
		gyro_xyz.y = 0.98 *(gyro_xyz.y+gyro_xyz.y*dt) + 0.02*accel_xyz.y;
		gyro_xyz.z = 0.98 *(gyro_xyz.z+gyro_xyz.z*dt) + 0.02*accel_xyz.z;
		}
*/
		pos = 0;
		hex_to_ascii((unsigned char*)&heading, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		hex_to_ascii((unsigned char*)&Pitch, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		hex_to_ascii((unsigned char*)&Roll, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		outbuff[pos++] = '\r';
		outbuff[pos++] = '\n';

		VCP_write(outbuff, pos);
	}

	ExpLedOff(ORANGE_LED);
}
Exemplo n.º 15
0
void FreeIMUBaseInt(char count)
{
	char outbuff[100];
	char i;
	Point3df accel_xyz, gyro_xyz, mag_xyz;
	int pos=0;
	int16_t ival;

	ExpLedOn(ORANGE_LED);

	for(i=0; i<count; i++){
		ExpLedToggle(ORANGE_LED);

		accelerometer_read(&accel_xyz);
		gyro_read(&gyro_xyz);
		MagPointRaw(&mag_xyz);

		pos = 0;
		ival = (int16_t)accel_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)accel_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)accel_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		outbuff[pos++] = '\r';
		outbuff[pos++] = '\n';

		VCP_write(outbuff, pos);
	}

	ExpLedOff(ORANGE_LED);
}
Exemplo n.º 16
0
Arquivo: main.c Projeto: 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);
		}
	}
}