diff --git a/conf/airframes/TUDELFT/tudelft_KM_conf.xml b/conf/airframes/TUDELFT/tudelft_KM_conf.xml index 6ee67f3029..af194ba168 100644 --- a/conf/airframes/TUDELFT/tudelft_KM_conf.xml +++ b/conf/airframes/TUDELFT/tudelft_KM_conf.xml @@ -18,7 +18,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml settings/estimation/body_to_imu.xml" - settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/video_thread.xml modules/cv_opticflow.xml modules/video_rtp_stream.xml" + settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/video_thread.xml modules/cv_opticflow.xml" gui_color="blue" /> - + - - - - - - - + + + + + + + + diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c index 1850186276..196fea41e4 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c @@ -178,7 +178,7 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra uint8_t SHIFT_TOO_FAR = 0; - memset(displacement, 0, size); + memset(displacement, 0, sizeof(int32_t)*size); int32_t border[2]; @@ -299,8 +299,8 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_ * @param[in] Displacement Pixel wise Displacement array * @param[in] *edge_hist_x Horizontal edge_histogram */ -void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement, - int32_t *edge_hist_x) +void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev + , int32_t *edge_hist_x) { struct point_t point1; struct point_t point2; @@ -310,25 +310,25 @@ void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct struct point_t point2_extra; uint16_t i; - for (i = 120; i < 240; i++) { + for (i = 1; i < img->w - 1; i++) { point1.y = -(uint16_t)edge_hist_x[i] / 100 + img->h / 3; point1.x = i; point2.y = -(uint16_t)edge_hist_x[i + 1] / 100 + img->h / 3; point2.x = i + 1; - point1_prev.y = -(uint16_t)displacement.x[i] * 5 + img->h * 2 / 3; + point1_prev.y = -(uint16_t)edge_hist_x_prev[i] / 100 + img->h * 2 / 3; point1_prev.x = i; - point2_prev.y = -(uint16_t)displacement.x[i + 1] * 5 + img->h * 2 / 3; + point2_prev.y = -(uint16_t)edge_hist_x_prev[i + 1] / 100 + img->h * 2 / 3; point2_prev.x = i + 1; image_draw_line(img, &point1, &point2); image_draw_line(img, &point1_prev, &point2_prev); } - point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * -180) / 100 + img->h / 2; + point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2; point1_extra.x = 0; - point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * 180) / 100 + img->h / 2; - point2_extra.x = 360; + point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2; + point2_extra.x = img->w; image_draw_line(img, &point1_extra, &point2_extra); } diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h index 543541b7cd..032608996e 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h @@ -85,8 +85,8 @@ struct edge_flow_t { // Local functions of the EDGEFLOW algorithm -void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement, - int32_t *edge_hist_x); +void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev + ,int32_t *edge_hist_x); void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr, uint8_t *previous_frame_offset, uint8_t *previous_frame_nr); void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 33ab872066..159b5a59b4 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -161,7 +161,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h) opticflow->prev_theta = 0.0; /* Set the default values */ - opticflow->method = 0; //0 = LK_fast9, 1 = Edgeflow + opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow opticflow->window_size = OPTICFLOW_WINDOW_SIZE; opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE; opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON @@ -489,7 +489,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * result->vel_body_y = vel_x; #if OPTICFLOW_SHOW_FLOW - draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x) + draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x); #endif // Increment and wrap current time frame current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index c7e3720eec..8a05c56b73 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -152,11 +152,10 @@ struct image_t *opticflow_module_calc(struct image_t *img) memcpy(&temp_state, &opticflow_state, sizeof(struct opticflow_state_t)); // Do the optical flow calculation - struct opticflow_result_t temp_result; - opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result); + opticflow_calc_frame(&opticflow, &temp_state, img, &opticflow_result); // Copy the result if finished - memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t)); +// memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t)); opticflow_got_result = true; return img;