/*
send OPTICAL_FLOW message
*/
void GCS_MAVLINK::send_opticalflow(AP_AHRS_NavEKF &ahrs, const OpticalFlow &optflow)
{
// exit immediately if no optical flow sensor or not healthy
if (!optflow.healthy()) {
return;
}
// get rates from sensor
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
float hagl = 0;
if (ahrs.have_inertial_nav()) {
ahrs.get_NavEKF().getHAGL(hagl);
}
// populate and send message
mavlink_msg_optical_flow_send(
chan,
hal.scheduler->millis(),
0, // sensor id is zero
flowRate.x,
flowRate.y,
bodyRate.x,
bodyRate.y,
optflow.quality(),
hagl); // ground distance (in meters) set to zero
}
void
l_optical_flow(const struct listener *l)
{
struct optical_flow_s flow;
orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
}
void send(const hrt_abstime t)
{
if (flow_sub->update(t)) {
mavlink_msg_optical_flow_send(_channel,
flow->timestamp,
flow->sensor_id,
flow->flow_raw_x, flow->flow_raw_y,
flow->flow_comp_x_m, flow->flow_comp_y_m,
flow->quality,
flow->ground_distance_m);
}
}
void send(const hrt_abstime t)
{
struct optical_flow_s flow;
if (flow_sub->update(&flow_time, &flow)) {
mavlink_msg_optical_flow_send(_channel,
flow.timestamp,
flow.sensor_id,
flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m,
flow.quality,
flow.ground_distance_m);
}
}
/**
* @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);
}
}
}
/**
* @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 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);
}
}
}
