diff --git a/conf/airframes/TUDELFT/tudelft_KM_conf.xml b/conf/airframes/TUDELFT/tudelft_KM_conf.xml
index af194ba168..73ae89e176 100644
--- a/conf/airframes/TUDELFT/tudelft_KM_conf.xml
+++ b/conf/airframes/TUDELFT/tudelft_KM_conf.xml
@@ -7,7 +7,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_att_int_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/estimation/body_to_imu.xml"
- settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml modules/opticflow_hover.xml"
+ settings_modules="modules/gps_ubx_ucenter.xml modules/video_thread.xml modules/cv_opticflow.xml"
gui_color="red"
/>
-
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
similarity index 97%
rename from conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml
rename to conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
index d3e63b3cef..200f2bb0ac 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop2_indi_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml
@@ -40,14 +40,10 @@
-
-
+
-
+
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 ef2f710020..ce5ee94273 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
@@ -42,7 +42,7 @@
#ifndef MAX_HORIZON
-#define MAX_HORIZON 10
+#define MAX_HORIZON 2
#else
#if MAX_HORIZON < 2
#define MAX_HORIZON 2
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index 481a1c78e0..aa28902769 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -120,7 +120,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE)
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
#ifndef OPTICFLOW_FAST9_MIN_DISTANCE
-#define OPTICFLOW_FAST9_MIN_DISTANCE 10
+#define OPTICFLOW_FAST9_MIN_DISTANCE 0
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE)
@@ -138,6 +138,10 @@ PRINT_CONFIG_VAR(OPTICFLOW_METHOD)
#endif
PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION)
+#ifndef CAMERA_ROTATED_180
+#define CAMERA_ROTATED 0
+#endif
+PRINT_CONFIG_VAR(CAMERA_ROTATED_180)
/* Functions only used here */
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
@@ -187,7 +191,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
- if (opticflow->just_switched_method == 1) {
+ if (opticflow->just_switched_method) {
+ printf("OPTICALFLOW: switched to fast9 Lukas Kanade\n");
opticflow_calc_init(opticflow, img->w, img->h);
}
@@ -214,7 +219,10 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// Corner detection
// *************************************************************************************
- // FAST corner detection (TODO: non fixed threshold)
+ // FAST corner detection
+ // TODO: non fixed threshold
+ // TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
+ // to 0 (see defines), however a more permanent solution should be considered
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
0, 0, &result->corner_cnt);
@@ -335,10 +343,6 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
// result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
// result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
- // Rotate velocities from camera frame coordinates to body coordinates.
- // IMPORTANT for control! This the case on the ARDrone and bebop, but on other systems this might be different!
- result->vel_body_x = vel_y;
- result->vel_body_y = - vel_x;
// Determine quality of noise measurement for state filter
//TODO Experiment with multiple noise measurement models
@@ -380,6 +384,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
// If the methods just switched to this one, reintialize the
// array of edge_hist structure.
if (opticflow->just_switched_method == 1) {
+ printf("OPTICALFLOW: switched to EdgeFlow\n");
int i;
for (i = 0; i < MAX_HORIZON; i++) {
edge_hist[i].x = malloc(sizeof(int32_t) * img->w);
@@ -500,14 +505,6 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
result->vel_x = vel_x;
result->vel_y = vel_y;
- /* Rotate velocities from camera frame coordinates to body coordinates.
- * IMPORTANT This frame to body orientation should be the case for the Parrot
- * ARdrone and Bebop, however this can be different for other quadcopters
- * ALWAYS double check!
- */
- result->vel_body_x = - vel_y;
- result->vel_body_y = vel_x;
-
#if OPTICFLOW_SHOW_FLOW
draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x);
#endif
@@ -526,14 +523,18 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
- static int8_t switch_counter = OPTICFLOW_METHOD;
- if (switch_counter != opticflow->method || opticflow->got_first_img) {
- opticflow->just_switched_method = 1;
+
+ // A switch counter that checks in the loop if the current method is similar,
+ // to the previous (for reinitializing structs)
+ static int8_t switch_counter = -1;
+ if (switch_counter != opticflow->method) {
+ opticflow->just_switched_method = true;
switch_counter = opticflow->method;
} else {
- opticflow->just_switched_method = 0;
+ opticflow->just_switched_method = false;
}
+ // Switch between methods (0 = fast9/lukas-kanade, 1 = EdgeFlow)
if (opticflow->method == 0) {
calc_fast9_lukas_kanade(opticflow, state, img, result);
} else {
@@ -542,10 +543,6 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
} else {}
}
- if (opticflow->got_first_img == 1) {
- opticflow->got_first_img = 0;
- }
-
}
/**
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index c494575fc1..82ab55f589 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -95,9 +95,10 @@ void opticflow_module_init(void)
// Initialize the opticflow calculation
opticflow_got_result = false;
- opticflow.got_first_img = 1;
cv_add(opticflow_module_calc);
+
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
#endif
@@ -122,7 +123,7 @@ void opticflow_module_run(void)
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
- opticflow_result.flow_der_x,
+ opticflow_result.flow_der_y,
quality,
opticflow_result.div_size,
opticflow_state.agl);
@@ -142,7 +143,9 @@ void opticflow_module_run(void)
/**
* The main optical flow calculation thread
* This thread passes the images trough the optical flow
- * calculator based on Lucas Kanade
+ * calculator
+ * @param[in] *img The image_t structure of the captured image
+ * @return *img The processed image structure
*/
struct image_t *opticflow_module_calc(struct image_t *img)
{
@@ -154,9 +157,21 @@ struct image_t *opticflow_module_calc(struct image_t *img)
opticflow_calc_frame(&opticflow, &opticflow_state, img, &opticflow_result);
// Copy the result if finished
-// memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
opticflow_got_result = true;
+ /* Rotate velocities from camera frame coordinates to body coordinates for control
+ * IMPORTANT!!! This frame to body orientation should be the case for the Parrot
+ * ARdrone and Bebop, however this can be different for other quadcopters
+ * ALWAYS double check!
+ */
+#if CAMERA_ROTATED_180 == 0 //Case for ARDrone 2.0
+ result->vel_body_x = result->vel_y;
+ result->vel_body_y = - result ->vel_x;
+#else // Case for Bebop 2
+ result->vel_body_x = - result->vel_y;
+ result->vel_body_y = result ->vel_x;
+#endif
+
return img;
}
@@ -170,7 +185,5 @@ static void opticflow_agl_cb(uint8_t sender_id __attribute__((unused)), float di
// Update the distance if we got a valid measurement
if (distance > 0) {
opticflow_state.agl = distance;
-
-
}
}