// TODO: why doesn't the above function write these structs directly?
void result_accumulator_fill_i2c_data(result_accumulator_ctx *ctx, i2c_frame* f, i2c_integral_frame* f_integral) {
result_accumulator_output_flow output_flow;
result_accumulator_output_flow_i2c output_i2c;
int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO];
result_accumulator_calculate_output_flow(ctx, min_valid_ratio, &output_flow);
result_accumulator_calculate_output_flow_i2c(ctx, min_valid_ratio, &output_i2c);
/* write the i2c_frame */
f->frame_count = ctx->frame_count;
f->pixel_flow_x_sum = output_flow.flow_x;
f->pixel_flow_y_sum = output_flow.flow_y;
f->flow_comp_m_x = floor(output_flow.flow_comp_m_x * 1000.0f + 0.5f);
f->flow_comp_m_y = floor(output_flow.flow_comp_m_y * 1000.0f + 0.5f);
f->qual = output_flow.quality;
f->ground_distance = output_i2c.ground_distance;
f->gyro_x_rate = output_i2c.gyro_x;
f->gyro_y_rate = output_i2c.gyro_y;
f->gyro_z_rate = output_i2c.gyro_z;
f->gyro_range = getGyroRange();
if (output_i2c.time_delta_distance_us < 255 * 1000) {
f->distance_timestamp = output_i2c.time_delta_distance_us / 1000; //convert to ms
} else {
f->distance_timestamp = 255;
}
/* write the i2c_integral_frame */
f_integral->frame_count_since_last_readout = output_i2c.valid_frames;
f_integral->gyro_x_rate_integral = output_i2c.integrated_gyro_x; //mrad*10
f_integral->gyro_y_rate_integral = output_i2c.integrated_gyro_y; //mrad*10
f_integral->gyro_z_rate_integral = output_i2c.integrated_gyro_z; //mrad*10
f_integral->pixel_flow_x_integral = output_i2c.rad_flow_x; //mrad*10
f_integral->pixel_flow_y_integral = output_i2c.rad_flow_y; //mrad*10
f_integral->integration_timespan = output_i2c.integration_time; //microseconds
f_integral->ground_distance = output_i2c.ground_distance; //mmeters
f_integral->distance_timestamp = output_i2c.time_delta_distance_us; //microseconds
f_integral->qual = output_i2c.quality; //0-255 linear quality measurement 0=bad, 255=best
f_integral->gyro_temperature = output_i2c.temperature; //Temperature * 100 in centi-degrees Celsius
}
/**
* @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();
}
}
}
