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);
}
}
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);
}
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));
}
/**
* @brief Main function.
*/
int main(void)
{
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 1000))
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
/* main loop */
while (1)
{
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(global_data.param[PARAM_CALIBRATION_ON])
{
while(global_data.param[PARAM_CALIBRATION_ON])
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor);
/* gyroscope coordinate transformation */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* calculate focal_length in pixel */
const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/* debug */
float x_rate_pixel = x_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
float y_rate_pixel = y_rate * (get_time_between_images() / 1000.0f) * focal_length_px;
//FIXME for the old sensor PX4FLOW v1.2 uncomment this!!!!
// x_rate = x_rate_raw_sensor; // change x and y rates
// y_rate = y_rate_raw_sensor;
/* get sonar data */
sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* compute optical flow */
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
if (sonar_distance_filtered > 5.0f || sonar_distance_filtered == 0.0f)
{
/* distances above 5m are considered invalid */
sonar_distance_filtered = 0.0f;
distance_valid = false;
}
else
{
distance_valid = true;
}
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000.0f);
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
valid_frame_count++;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
/* TODO for debugging */
//mavlink_msg_named_value_float_send(MAVLINK_COMM_2, boot_time_ms, "blabla", blabla);
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
if (counter % 2 == 0)
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
float ground_distance = 0.0f;
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
if(global_data.param[PARAM_SONAR_FILTERED])
ground_distance = sonar_distance_filtered;
else
ground_distance = sonar_distance_raw;
if (valid_frame_count > 0)
{
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, flow_comp_m_x, flow_comp_m_y, qual,
ground_distance, x_rate, y_rate, z_rate);
}
else
{
// send distance
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW])
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_ms() * 1000, global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
0.0f, 0.0f, 0, ground_distance);
update_TX_buffer(pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f, 0.0f, 0.0f, 0, ground_distance, x_rate, y_rate,
z_rate);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_ms() * 1000, x_rate, y_rate, z_rate);
}
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* transmit raw 8-bit image */
if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
if (global_data.param[PARAM_VIDEO_USB_MODE] == CAM_VIDEO)
{
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size_send / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * 253]);
}
}
else if (global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO)
{
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / 253 + 1,
253,
100);
LEDToggle(LED_COM);
uint16_t frame;
for (frame = 0; frame < image_size / 253 + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * 253]);
}
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
}
void 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
}
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;
}
/**
* @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();
}
}
}
/**
* @brief Main function.
*/
int main(void)
{
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
uint8_t current_image1[64][64];
uint8_t current_image2[64][64];
uint8_t current_image3[64][64];
uint8_t current_image4[64][64];
uint8_t previous_image1[64][64];
uint8_t previous_image2[64][64];
uint8_t previous_image3[64][64];
uint8_t previous_image4[64][64];
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
current_image1[i][j]=0;
previous_image1[i][j]=0;
current_image2[i][j]=0;
previous_image2[i][j]=0;
current_image3[i][j]=0;
previous_image3[i][j]=0;
current_image4[i][j]=0;
previous_image4[i][j]=0;
}
}
uint8_t ourImage[OurSize];
for(int i=0;i<OurSize;++i)
{
ourImage[i]=0;
}
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
float pixel_flow_x_lp = 0.0f;
float pixel_flow_y_lp = 0.0f;
static float accumulated_flow_x = 0;
static float accumulated_flow_y = 0;
static float accumulated_gyro_x = 0;
static float accumulated_gyro_y = 0;
static float accumulated_gyro_z = 0;
static uint16_t accumulated_framecount = 0;
static uint16_t accumulated_quality = 0;
static uint32_t integration_timespan = 0;
static uint32_t lasttime = 0;
uint32_t time_since_last_sonar_update= 0;
static float last_vel_x = 0;
static float last_vel_y = 0;
float vel_x = 0, vel_y = 0;
//Change
int count=1;
//int gyroCount=1;
uint8_t * tmp_image1 = *previous_image;
uint8_t * tmp_image2 = *current_image;
float x_temprate=0;
float y_temprate=0;
float z_temprate=0;
int defCount=3;
for (uint16_t pixel = 0; pixel < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
uint16_t image_sum1=0;
uint16_t image_sum2=0;
uint16_t image_sum3=0;
uint16_t image_sum4=0;
//EndChange
/* main loop */
while (1)
{
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(global_data.param[PARAM_VIDEO_ONLY])
{
while(global_data.param[PARAM_VIDEO_ONLY])
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
//StartChange
if(count<=defCount)
{
if(count==1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum1+=*(previous_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum2+=*(previous_image+pixel);
}
if(image_sum1<image_sum2)
{
image_sum1=image_sum2;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum2=0;
}
}
if(count>defCount&&count<=2*defCount)
{
// uint16_t image_sum1=0;
// uint16_t image_sum2=0;
if(count==defCount+1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum3+=*(current_image+pixel);
}
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
image_sum4+=*(current_image+pixel);
}
if(image_sum3<image_sum4)
{
image_sum3=image_sum4;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image2+pixel)=(uint8_t)(*(previous_image+pixel));
}
}
image_sum4=0;
}
}
// if(count<=defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image1+pixel)+=(uint8_t)(*(previous_image+pixel)/defCount);
// }
// }
// if(count>defCount&&count<=2*defCount)
// {
// for (uint16_t pixel = 0; pixel < image_size; pixel++)
// {
// *(tmp_image2+pixel)+=(uint8_t)(*(current_image+pixel)/defCount);
// }
//
// }
count++;
if(count==2*defCount+1)
{
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation */
x_temprate += y_rate_sensor/(2*defCount); // change x and y rates
y_temprate += - x_rate_sensor/(2*defCount);
z_temprate += z_rate_sensor/(2*defCount); // z is correct
/* calculate focal_length in pixel */
const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
float x_rate = x_temprate; // change x and y rates
float y_rate = - y_temprate;
float z_rate = z_temprate; // z is correct
x_temprate =0.0f;
y_temprate =0.0f;
z_temprate =0.0f;
// /* calculate focal_length in pixel */
// const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
//
// /* get sonar data */
// distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
*previous_image=tmp_image1;
*current_image=tmp_image2;
count=1;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
{
*(tmp_image1+pixel)=0;
*(tmp_image2+pixel)=0;
}
image_sum1=0;
image_sum2=0;
image_sum3=0;
image_sum4=0;
/* compute optical flow */
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
for(int i=0;i<64;++i)
{
for(int j=0;j<64;++j)
{
previous_image1[i][j]=*(previous_image+64*i+j);
current_image1[i][j]=*(current_image+64*i+j);
}
}
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
valid_frame_count++;
uint32_t deltatime = (get_boot_time_us() - lasttime);
integration_timespan += deltatime;
accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad
accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad
accumulated_framecount++;
accumulated_quality += qual;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
//update lasttime
lasttime = get_boot_time_us();
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
if(global_data.param[PARAM_SENSOR_POSITION] == BOTTOM)
{
/* send bottom flow if activated */
float ground_distance = 0.0f;
if(global_data.param[PARAM_SONAR_FILTERED])
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
/* lowpass velocity output */
pixel_flow_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_x_lp;
pixel_flow_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * pixel_flow_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * pixel_flow_y_lp;
}
else
{
pixel_flow_x_lp = pixel_flow_x;
pixel_flow_y_lp = pixel_flow_y;
}
//update I2C transmitbuffer
if(valid_frame_count>0)
{
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
last_vel_x = velocity_x_sum/valid_frame_count;
last_vel_y = velocity_y_sum/valid_frame_count;
vel_x = last_vel_x;
vel_y = last_vel_y;
}
else
{
/*
update_TX_buffer(pixel_flow_x_lp, pixel_flow_y_lp, last_vel_x, last_vel_y, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp);
vel_x = 0;
vel_y = 0;
*/
}
//serial mavlink + usb mavlink output throttled
if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
if(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED])
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
if(valid_frame_count>0)
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
else
{
flow_comp_m_x = 0.0f;
flow_comp_m_y = 0.0f;
}
}
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
/*
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
vel_x, vel_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
*/
/*
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
last_vel_x, last_vel_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
*/
/* send approximate local position estimate without heading */
if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
{
/* rough local position estimate for unit testing */
lpos.x += ground_distance*accumulated_flow_x;
lpos.y += ground_distance*accumulated_flow_y;
lpos.z = -ground_distance;
/* velocity not directly measured and not important for testing */
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
} else {
/* toggling param allows user reset */
lpos.x = 0;
lpos.y = 0;
lpos.z = 0;
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
}
if (global_data.param[PARAM_USB_SEND_FLOW])
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
integration_timespan = 0;
accumulated_flow_x = 0;
accumulated_flow_y = 0;
accumulated_framecount = 0;
accumulated_quality = 0;
accumulated_gyro_x = 0;
accumulated_gyro_y = 0;
accumulated_gyro_z = 0;
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (global_data.param[PARAM_SYSTEM_SEND_STATE])
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* send local position estimate, for testing only, doesn't account for heading */
if (send_lpos_now)
{
if (global_data.param[PARAM_SYSTEM_SEND_LPOS])
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
}
send_lpos_now = false;
}
/* transmit raw 8-bit image */
if (global_data.param[PARAM_USB_SEND_VIDEO] && send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
100);
LEDToggle(LED_COM);
uint16_t frame = 0;
for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
}
send_image_now = false;
}
else if (!global_data.param[PARAM_USB_SEND_VIDEO])
{
LEDOff(LED_COM);
}
}
//EndChange
}
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_ground_car(void)
{
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_observer_correct_accel(global_data.accel_raw);
// Write in roll and pitch
static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
attitude_observer_get_angles(&att);
global_data.attitude.x = att.x;
global_data.attitude.y = att.y;
if (global_data.param[PARAM_ATT_KAL_IYAW])
{
global_data.attitude.z += 0.005 * global_data.gyros_si.z;
}
else
{
global_data.attitude.z = att.z;
}
// Prediction step of observer
attitude_observer_predict(global_data.gyros_si);
// TODO READ OUT MOUSE SENSOR
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read Analog-to-Digital converter
adc_read();
// Read remote control
remote_control();
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// Send buffered data such as debug text messages
communication_queued_send();
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
//led_toggle(LED_YELLOW);
led_toggle(LED_RED); // just for green LED on alpha at the moment
// Toggle active mode led
if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
communication_send_attitude_position(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else
{
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
if (loop_start_time - last_mainloop_idle >= 5000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
void main_loop_fixed_wing(void)
{
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_get_time_usec();
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
if (us_run_every(2500, COUNTER1, loop_start_time))
{
camera_shutter_handling(loop_start_time);
}
if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING)
{
///////////////////////////////////////////////////////////////////////////
/// RC INTERFACE HACK AT 50 Hz
///////////////////////////////////////////////////////////////////////////
if (us_run_every(20000, COUNTER8, loop_start_time))
{
// Write start byte
uart1_transmit(0xFF);
// Write channels 1-6
for (int i = 1; i < 7; i++)
{
uart1_transmit((radio_control_get_channel(1)+1)*127);
}
}
led_toggle(LED_GREEN);
led_toggle(LED_RED);
// Do not execute any of the functions below
continue;
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_observer_correct_accel(global_data.accel_raw);
// Write in roll and pitch
static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
attitude_observer_get_angles(&att);
global_data.attitude.x = att.x;
global_data.attitude.y = att.y;
if (global_data.param[PARAM_ATT_KAL_IYAW])
{
global_data.attitude.z += 0.005 * global_data.gyros_si.z;
}
else
{
global_data.attitude.z = att.z;
}
// Prediction step of observer
attitude_observer_predict(global_data.gyros_si);
control_fixed_wing_attitude();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read Analog-to-Digital converter
adc_read();
// Read remote control
remote_control();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(10000, COUNTER6, loop_start_time))
{
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
communication_queued_send();
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
//led_toggle(LED_YELLOW);
led_toggle(LED_RED); // just for green LED on alpha at the moment
// Toggle active mode led
if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode
== MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN],
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else {
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time);
if (loop_start_time - last_mainloop_idle >= 5000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
void main_loop_quadrotor(void)
{
/**
* @brief Initialize the whole system
*
* All functions that need to be called before the first mainloop iteration
* should be placed here.
*/
main_init_generic();
control_quadrotor_position_init();
control_quadrotor_attitude_init();
attitude_tobi_laurens_init();
// FIXME XXX Make proper mode switching
// outdoor_position_kalman_init();
//vision_position_kalman_init();
// Default filters, allow Vision, Vicon and optical flow inputs
vicon_position_kalman_init();
optflow_speed_kalman_init();
/**
* @brief This is the main loop
*
* It will be executed at maximum MCU speed (60 Mhz)
*/
// Executiontime debugging
time_debug.x = 0;
time_debug.y = 0;
time_debug.z = 0;
last_mainloop_idle = sys_time_clock_get_time_usec();
debug_message_buffer("Starting main loop");
led_off(LED_GREEN);
led_off(LED_RED);
while (1)
{
// Time Measurement
uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore
///////////////////////////////////////////////////////////////////////////
/// CRITICAL 200 Hz functions
///////////////////////////////////////////////////////////////////////////
if (us_run_every(5000, COUNTER2, loop_start_time))
{
// Kalman Attitude filter, used on all systems
gyro_read();
sensors_read_acc();
sensors_pressure_bmp085_read_out();
// Read out magnetometer at its default 50 Hz rate
static uint8_t mag_count = 0;
if (mag_count == 3)
{
sensors_read_mag();
//attitude_observer_correct_magnet(global_data.magnet_corrected);
mag_count = 0;
}else if(mag_count==1){
hmc5843_start_read();
mag_count++;
}
else
{
mag_count++;
}
// Correction step of observer filter
attitude_tobi_laurens();
if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY ||
global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP)
{
vicon_position_kalman();
}
else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY)
{
outdoor_position_kalman();
}
control_quadrotor_attitude();
//debug counting number of executions
count++;
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Camera Shutter - This takes 50 usecs!!!
///////////////////////////////////////////////////////////////////////////
// Set camera shutter with 2.5ms resolution
else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!!
{
camera_shutter_handling(loop_start_time);
// Measure time for debugging
time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec()
- loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 50 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(20000, COUNTER3, loop_start_time))
{
// Read infrared sensor
//adc_read();
// Control the quadrotor position
control_quadrotor_position();
// Read remote control
remote_control();
control_camera_angle();
// //float_vect3 opt;
// static float_vect3 opt_int;
// uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z);
// if (valid)
// {
// opt_int.x += global_data.optflow.x;
// opt_int.y += global_data.optflow.y;
//
// }
//
// uint8_t supersampling = 10;
// for (int i = 0; i < supersampling; ++i)
// {
// global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL);
// }
//
// global_data.sonar_distance /= supersampling;
//
// opt_int.z = valid;
// static unsigned int i = 0;
// if (i == 10)
// {
// mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered);
//
// i = 0;
// }
// i++;
//optical_flow_debug_vect_send();
//debug_vect("opt_int", opt_int);
// optical_flow_start_read(80);
if (global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION
|| global_data.state.position_estimation_mode
== POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION)
{
optflow_speed_kalman();
}
// Send the raw sensor/ADC values
communication_send_raw_data(loop_start_time);
float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f;
debug_vect("yaw_low", yy);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// CRITICAL FAST 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER4, loop_start_time))
{
//*** this happens in handle_controller_timeouts already!!!!! ***
// //update global_data.state
// if (global_data.param[PARAM_VICON_MODE] == 1)
// {
// //VICON_MODE 1 only accepts vicon position
// global_data.state.position_fix = global_data.state.vicon_ok;
// }
// else
// {
// //VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision
// global_data.state.position_fix = global_data.state.vision_ok;
// }
update_system_statemachine(loop_start_time);
update_controller_setpoints();
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_loop_start_time_boot_ms(),
global_data.attitude_setpoint.x,
global_data.attitude_setpoint.y,
global_data.position_yaw_control_output,
global_data.thrust_control_output);
//STARTING AND LANDING
quadrotor_start_land_handler(loop_start_time);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 100 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER6, loop_start_time))
{
if (global_data.param[PARAM_SEND_SLOT_DEBUG_6])
{
debug_vect("att_ctrl_o", global_data.attitude_control_output);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// UNCRITICAL SLOW 5 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(200000, COUNTER8, loop_start_time))
{
// The onboard controllers go into failsafe mode once
// position data is missing
handle_controller_timeouts(loop_start_time);
// Send buffered data such as debug text messages
// Empty one message out of the buffer
debug_message_send_one();
// Toggle status led
led_toggle(LED_RED);
// Toggle active mode led
if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED)
{
led_on(LED_GREEN);
}
else
{
led_off(LED_GREEN);
}
handle_eeprom_write_request();
handle_reset_request();
update_controller_parameters();
communication_send_controller_feedback();
communication_send_remote_control();
// Pressure sensor driver works, but not tested regarding stability
// sensors_pressure_bmp085_read_out();
if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1)
{
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
90, global_data.param[PARAM_POSITION_SETPOINT_YAW]);
mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0,
91, global_data.yaw_pos_setpoint);
}
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 1 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(1000000, COUNTER9, loop_start_time))
{
// Send system state, mode, battery voltage, etc.
send_system_state();
// Send position setpoint offset
//debug_vect("pos offs", global_data.position_setpoint_offset);
// Send current onboard time
mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms());
//update state from received parameters
sync_state_parameters();
//debug number of execution
count = 0;
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//Send GPS information
float_vect3 gps;
gps.x = gps_utm_north / 100.0f;//m
gps.y = gps_utm_east / 100.0f;//m
gps.z = gps_utm_zone;// gps_week;
debug_vect("GPS", gps);
}
else if (global_data.param[PARAM_GPS_MODE] == 9
|| global_data.param[PARAM_GPS_MODE] == 8)
{
if (global_data.param[PARAM_GPS_MODE] == 8)
{
gps_set_local_origin();
// gps_local_home_init = false;
}
if (gps_lat == 0)
{
debug_message_buffer("GPS Signal Lost");
}
else
{
float_vect3 gps_local, gps_local_velocity;
gps_get_local_position(&gps_local);
debug_vect("GPS local", gps_local);
gps_get_local_velocity(&gps_local_velocity);
debug_vect("GPS loc velocity", gps_local_velocity);
}
}
if (global_data.state.gps_mode)
{
gps_send_local_origin();
}
beep_on_low_voltage();
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 20 Hz functions
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(50000, COUNTER7, loop_start_time))
{
//led_toggle(LED_YELLOW);
if (global_data.param[PARAM_GPS_MODE] >= 10)
{
//get thru all gps messages
debug_message_send_one();
}
communication_send_attitude_position(loop_start_time);
// Send parameter
communication_queued_send();
// //infrared distance
// float_vect3 infra;
// infra.x = global_data.ground_distance;
// infra.y = global_data.ground_distance_unfiltered;
// infra.z = global_data.state.ground_distance_ok;
// debug_vect("infrared", infra);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// NON-CRITICAL SLOW 200 Hz functions //
///////////////////////////////////////////////////////////////////////////
else if (us_run_every(5000, COUNTER5, loop_start_time))
{
if (global_data.state.status == MAV_STATE_STANDBY)
{
//Check if parameters should be written or read
param_handler();
}
}
///////////////////////////////////////////////////////////////////////////
else
{
// All Tasks are fine and we have no starvation
last_mainloop_idle = loop_start_time;
}
// Read out comm at max rate - takes only a few microseconds in worst case
communication_receive();
// MCU load measurement
uint64_t loop_stop_time = sys_time_clock_get_time_usec();
global_data.cpu_usage = measure_avg_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
global_data.cpu_peak = measure_peak_cpu_load(loop_start_time,
loop_stop_time, min_mainloop_time);
time_debug.y = max(time_debug.y, global_data.cpu_usage);
time_debug.z = max(time_debug.z, global_data.cpu_peak);
if (loop_start_time - last_mainloop_idle >= 20000)
{
debug_message_buffer(
"CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!");
last_mainloop_idle = loop_start_time;//reset to prevent multiple messages
}
if (global_data.cpu_usage > 800)
{
// CPU load higher than 80%
debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%");
}
} // End while(1)
}
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
}
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);
}
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);
}
/**
* @brief Main function.
*/
int main(void)
{
__enable_irq();
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
PROBE_INIT();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
board_led_rgb(255,255,255, 1);
board_led_rgb( 0, 0,255, 0);
board_led_rgb( 0, 0, 0, 0);
board_led_rgb(255, 0, 0, 1);
board_led_rgb(255, 0, 0, 2);
board_led_rgb(255, 0, 0, 3);
board_led_rgb( 0,255, 0, 3);
board_led_rgb( 0, 0,255, 4);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init clock */
if (SysTick_Config(SystemCoreClock / 100000))/*set timer to trigger interrupt every 10 microsecond */
{
/* capture clock error */
LEDOn(LED_ERR);
while (1);
}
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* enable image capturing */
enable_image_capture();
/* gyro config */
gyro_config();
/* init and clear fast image buffers */
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
uint8_t * current_image = image_buffer_8bit_1;
uint8_t * previous_image = image_buffer_8bit_2;
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer[TIMER_SONAR] = SONAR_TIMER_COUNT;
timer[TIMER_SYSTEM_STATE] = SYSTEM_STATE_COUNT;
timer[TIMER_RECEIVE] = SYSTEM_STATE_COUNT / 2;
timer[TIMER_PARAMS] = PARAMS_COUNT;
timer[TIMER_IMAGE] = global_data.param[PARAM_VIDEO_RATE];
/* variables */
uint32_t counter = 0;
uint8_t qual = 0;
/* bottom flow variables */
float pixel_flow_x = 0.0f;
float pixel_flow_y = 0.0f;
float pixel_flow_x_sum = 0.0f;
float pixel_flow_y_sum = 0.0f;
float velocity_x_sum = 0.0f;
float velocity_y_sum = 0.0f;
float velocity_x_lp = 0.0f;
float velocity_y_lp = 0.0f;
int valid_frame_count = 0;
int pixel_flow_count = 0;
static float accumulated_flow_x = 0;
static float accumulated_flow_y = 0;
static float accumulated_gyro_x = 0;
static float accumulated_gyro_y = 0;
static float accumulated_gyro_z = 0;
static uint16_t accumulated_framecount = 0;
static uint16_t accumulated_quality = 0;
static uint32_t integration_timespan = 0;
static uint32_t lasttime = 0;
uint32_t time_since_last_sonar_update= 0;
uint32_t time_last_pub= 0;
uavcan_start();
/* main loop */
while (1)
{
PROBE_1(false);
uavcan_run();
PROBE_1(true);
/* reset flow buffers if needed */
if(buffer_reset_needed)
{
buffer_reset_needed = 0;
for (int i = 0; i < global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT]; i++)
{
image_buffer_8bit_1[i] = 0;
image_buffer_8bit_2[i] = 0;
}
delay(500);
continue;
}
/* calibration routine */
if(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
while(FLOAT_AS_BOOL(global_data.param[PARAM_VIDEO_ONLY]))
{
dcmi_restart_calibration_routine();
/* waiting for first quarter of image */
while(get_frame_counter() < 2){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for second quarter of image */
while(get_frame_counter() < 3){}
dma_copy_image_buffers(¤t_image, &previous_image, FULL_IMAGE_SIZE, 1);
/* waiting for all image parts */
while(get_frame_counter() < 4){}
send_calibration_image(&previous_image, ¤t_image);
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
communication_system_state_send();
communication_receive_usb();
debug_message_send_one();
communication_parameter_send();
LEDToggle(LED_COM);
}
dcmi_restart_calibration_routine();
LEDOff(LED_COM);
}
uint16_t image_size = global_data.param[PARAM_IMAGE_WIDTH] * global_data.param[PARAM_IMAGE_HEIGHT];
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* calculate focal_length in pixel */
const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
/* compute optical flow */
if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
{
/* copy recent image to faster ram */
dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1);
/* compute optical flow */
qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y);
/*
* real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
* x / f = X / Z
* y / f = Y / Z
*/
float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f);
float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f);
if (qual > 0)
{
valid_frame_count++;
uint32_t deltatime = (get_boot_time_us() - lasttime);
integration_timespan += deltatime;
accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis
accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad
accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad
accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad
accumulated_framecount++;
accumulated_quality += qual;
}
/* integrate velocity and output values only if distance is valid */
if (distance_valid)
{
/* calc velocity (negative of flow values scaled with distance) */
float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;
time_since_last_sonar_update = (get_boot_time_us()- get_sonar_measure_time());
if (qual > 0)
{
velocity_x_sum += new_velocity_x;
velocity_y_sum += new_velocity_y;
/* lowpass velocity output */
velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_y +
(1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
}
else
{
/* taking flow as zero */
velocity_x_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_x_lp;
velocity_y_lp = (1.0f - global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW]) * velocity_y_lp;
}
//update lasttime
lasttime = get_boot_time_us();
pixel_flow_x_sum += pixel_flow_x;
pixel_flow_y_sum += pixel_flow_y;
pixel_flow_count++;
}
counter++;
if (FLOAT_EQ_INT(global_data.param[PARAM_SENSOR_POSITION], BOTTOM))
{
/* send bottom flow if activated */
float ground_distance = 0.0f;
if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]))
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
uavcan_define_export(i2c_data, legacy_12c_data_t, ccm);
uavcan_define_export(range_data, range_data_t, ccm);
uavcan_timestamp_export(i2c_data);
uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc);
//update I2C transmitbuffer
if(valid_frame_count>0)
{
update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
}
else
{
update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual,
ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));
}
PROBE_2(false);
uavcan_publish(range, 40, range_data);
PROBE_2(true);
PROBE_3(false);
uavcan_publish(flow, 40, i2c_data);
PROBE_3(true);
//serial mavlink + usb mavlink output throttled
uint32_t now = get_boot_time_us();
uint32_t time_since_last_pub = now - time_last_pub;
if (time_since_last_pub > (1.0e6f/global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE]))
{
time_last_pub = now;
float flow_comp_m_x = 0.0f;
float flow_comp_m_y = 0.0f;
if(FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_LP_FILTERED]))
{
flow_comp_m_x = velocity_x_lp;
flow_comp_m_y = velocity_y_lp;
}
else
{
if(valid_frame_count>0)
{
flow_comp_m_x = velocity_x_sum/valid_frame_count;
flow_comp_m_y = velocity_y_sum/valid_frame_count;
}
else
{
flow_comp_m_x = 0.0f;
flow_comp_m_y = 0.0f;
}
}
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
/* send approximate local position estimate without heading */
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
{
/* rough local position estimate for unit testing */
lpos.x += ground_distance*accumulated_flow_x;
lpos.y += ground_distance*accumulated_flow_y;
lpos.z = -ground_distance;
lpos.vx = ground_distance*accumulated_flow_x/integration_timespan;
lpos.vy = ground_distance*accumulated_flow_y/integration_timespan;
lpos.vz = 0; // no direct measurement, just ignore
} else {
/* toggling param allows user reset */
lpos.x = 0;
lpos.y = 0;
lpos.z = 0;
lpos.vx = 0;
lpos.vy = 0;
lpos.vz = 0;
}
if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_FLOW]))
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
pixel_flow_x_sum * 10.0f, pixel_flow_y_sum * 10.0f,
flow_comp_m_x, flow_comp_m_y, qual, ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
integration_timespan, accumulated_flow_x, accumulated_flow_y,
accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z,
gyro_temp, accumulated_quality/accumulated_framecount,
time_since_last_sonar_update,ground_distance);
}
if(FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_GYRO]))
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
integration_timespan = 0;
accumulated_flow_x = 0;
accumulated_flow_y = 0;
accumulated_framecount = 0;
accumulated_quality = 0;
accumulated_gyro_x = 0;
accumulated_gyro_y = 0;
accumulated_gyro_z = 0;
velocity_x_sum = 0.0f;
velocity_y_sum = 0.0f;
pixel_flow_x_sum = 0.0f;
pixel_flow_y_sum = 0.0f;
valid_frame_count = 0;
pixel_flow_count = 0;
}
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
/* send system state, receive commands */
if (send_system_state_now)
{
/* every second */
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_STATE]))
{
communication_system_state_send();
}
send_system_state_now = false;
}
/* receive commands */
if (receive_now)
{
/* test every second */
communication_receive();
communication_receive_usb();
receive_now = false;
}
/* sending debug msgs and requested parameters */
if (send_params_now)
{
debug_message_send_one();
communication_parameter_send();
send_params_now = false;
}
/* send local position estimate, for testing only, doesn't account for heading */
if (send_lpos_now)
{
if (FLOAT_AS_BOOL(global_data.param[PARAM_SYSTEM_SEND_LPOS]))
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_2, timer_ms, lpos.x, lpos.y, lpos.z, lpos.vx, lpos.vy, lpos.vz);
}
send_lpos_now = false;
}
/* transmit raw 8-bit image */
if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])&& send_image_now)
{
/* get size of image to send */
uint16_t image_size_send;
uint16_t image_width_send;
uint16_t image_height_send;
image_size_send = image_size;
image_width_send = global_data.param[PARAM_IMAGE_WIDTH];
image_height_send = global_data.param[PARAM_IMAGE_HEIGHT];
mavlink_msg_data_transmission_handshake_send(
MAVLINK_COMM_2,
MAVLINK_DATA_STREAM_IMG_RAW8U,
image_size_send,
image_width_send,
image_height_send,
image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1,
MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN,
100);
LEDToggle(LED_COM);
uint16_t frame = 0;
for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++)
{
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]);
}
send_image_now = false;
}
else if (!FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]))
{
LEDOff(LED_COM);
}
}
}