diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_edgeflow.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_edgeflow.xml
index e85d4fb4ef..cb54263702 100644
--- a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_edgeflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_edgeflow.xml
@@ -29,8 +29,8 @@
-
-
+
+
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 664307fea9..6b1582bc0a 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c
@@ -20,7 +20,7 @@ void test_function(struct image_t *img,struct image_t *img_gray)
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
char direction, uint16_t edge_threshold)
{
- uint8_t *img_buf = (uint8_t *)img->buf;
+ uint8_t *img_buf = (uint8_t *)img->buf;
// TODO use arm_conv_q31()
int32_t sobel_sum = 0;
@@ -138,7 +138,7 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
}
}
if(!SHIFT_TOO_FAR)
- displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
+ displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
else
displacement[x]=0;
}
@@ -210,14 +210,46 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_
//return total_error / size;
}
+/**
+ * Calculate the difference from start till finish
+ * @param[in] *starttime The start time to calculate the difference from
+ * @param[in] *finishtime The finish time to calculate the difference from
+ * @return The difference in milliseconds
+ */
+ uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime)
+{
+ uint32_t msec;
+ msec = (finishtime->tv_sec - starttime->tv_sec) * 1000;
+ msec += (finishtime->tv_usec - starttime->tv_usec) / 1000;
+ return msec;
+}
void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
static struct edge_hist_t edge_hist[MAX_HORIZON];
- static uint8_t current_frame_nr = 1;
- struct edge_flow_t edgeflow;
+ static uint8_t current_frame_nr = 1;
+ struct edge_flow_t edgeflow;
+ static uint8_t previous_frame_offset[2] = {0,0};
+ static uint8_t counter_check = 0;
+
+ /*static uint32_t previous_time = 0;//sys_time.nb_tick;
+ static uint16_t freq_counter = 0;
+ static uint8_t frequency = 0;
+
+ freq_counter++;
+ if ((sys_time.nb_tick - previous_time) > sys_time.ticks_per_sec) { // 1s has past
+ frequency = (uint8_t)((freq_counter * (sys_time.nb_tick - previous_time)) / sys_time.ticks_per_sec);
+ freq_counter = 0;
+ previous_time = sys_time.nb_tick;
+ }
+ printf("freq %d\n",frequency);*/
+
+
+
+ // printf("freq %d\n",frequency);
+ uint8_t disp_range = DISP_RANGE_MAX;
int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal;
int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical;
@@ -225,20 +257,44 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
calculate_edge_histogram(img, edge_hist_x, 'x',0);
calculate_edge_histogram(img, edge_hist_y, 'y',0);
- uint8_t previous_frame_x = (current_frame_nr - 1 + MAX_HORIZON) %
+ //edge_hist[current_frame_nr].frame_time = img->ts;
+
+ memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
+
+ if (MAX_HORIZON > 2) {
+ uint32_t flow_mag_x, flow_mag_y;
+ flow_mag_x = abs(result->flow_x);
+ flow_mag_y = abs(result->flow_y);
+
+ uint32_t min_flow = 3;
+ uint32_t max_flow = disp_range - 3;
+ uint8_t previous_frame_offset_x = previous_frame_offset[0];
+ uint8_t previous_frame_offset_y = previous_frame_offset[1];
+
+ if (flow_mag_x > max_flow && previous_frame_offset_x > 1)
+ previous_frame_offset[0] = previous_frame_offset_x - 1;
+ if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1)
+ previous_frame_offset[0] = previous_frame_offset_x + 1;
+ if (flow_mag_y > max_flow && previous_frame_offset_y > 1)
+ previous_frame_offset[1] = previous_frame_offset_y - 1;
+ if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1)
+ previous_frame_offset[1] = previous_frame_offset_y + 1;
+ }
+
+
+ uint8_t previous_frame_x = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
MAX_HORIZON; // wrap index
- uint8_t previous_frame_y = (current_frame_nr - 1 + MAX_HORIZON) %
+ uint8_t previous_frame_y = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
MAX_HORIZON; // wrap index
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].horizontal;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].vertical;
struct edgeflow_displacement_t displacement;
- uint8_t disp_range = DISP_RANGE_MAX;
- calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
+ calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
displacement.horizontal, img->w,
opticflow->window_size, disp_range, 0);
- calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
+ calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
displacement.vertical, img->h,
opticflow->window_size, disp_range, 0);
@@ -252,10 +308,35 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
uint16_t i;
- result->flow_x = (int16_t)edgeflow.horizontal_flow/RES;
- result->flow_y = (int16_t)edgeflow.vertical_flow/RES;
+ result->flow_x = (int16_t)edgeflow.horizontal_flow/(previous_frame_offset[0]*RES);
+ result->flow_y = (int16_t)edgeflow.vertical_flow/(previous_frame_offset[1]*RES);
+ float fps_x = 0;
+ float fps_y = 0;
+ if(counter_check>MAX_HORIZON)
+ {
+ printf("%d\n",img->ts.tv_sec);
+ printf("%d %d %d %d \n",current_frame_nr,previous_frame_x, img->ts.tv_usec, edge_hist[previous_frame_x].frame_time.tv_usec);
+ printf("1: %d\n",timeval_diff2(&edge_hist[previous_frame_x].frame_time, &img->ts));
+ printf("2: %d\n",timeval_diff2(&edge_hist[previous_frame_y].frame_time, &img->ts));
+
+ float time_diff_x = (float)(timeval_diff2(&edge_hist[previous_frame_x].frame_time, &edge_hist[current_frame_nr].frame_time)) / 1000;
+ float time_diff_y = (float)(timeval_diff2(&edge_hist[previous_frame_y].frame_time, &edge_hist[current_frame_nr].frame_time)) / 1000;
+
+ fps_x = 1/(time_diff_x);
+ fps_y= 1/(time_diff_x);
+ result->fps = (fps_x + fps_y)/ 2;
+ }
+ else
+ counter_check++;
+
+ float vel_hor = edgeflow.horizontal_flow * fps_x* state->agl * OPTICFLOW_FOV_W / (img->w * RES);
+ float vel_ver = edgeflow.vertical_flow * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
+
+ result->vel_x = vel_ver;
+ result->vel_y = - vel_hor;
+
struct point_t point1;
struct point_t point2;
struct point_t point1_prev;
@@ -282,10 +363,10 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
}
point1_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * -180 )/ 100+ img->h/2;
- point1_extra.x = 0;
- point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2;
- point2_extra.x = 360;
- image_draw_line(img, &point1_extra,&point2_extra);
+ point1_extra.x = 0;
+ point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2;
+ point2_extra.x = 360;
+ image_draw_line(img, &point1_extra,&point2_extra);
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
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 3716a51fe5..775cfa9909 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
@@ -16,16 +16,26 @@
#include "opticflow/opticflow_calculator.h"
#include
#include
+#include
+
+#include "mcu_periph/sys_time.h"
#define MAX_HORIZON 10
#define IMAGE_HEIGHT 240
#define IMAGE_WIDTH 320
#define DISP_RANGE_MAX 20
+#ifndef OPTICFLOW_FOV_W
+#define OPTICFLOW_FOV_W 0.89360857702
+#endif
+
+#ifndef OPTICFLOW_FOV_H
+#define OPTICFLOW_FOV_H 0.67020643276
+#endif
struct edge_hist_t {
int32_t horizontal[IMAGE_WIDTH];
int32_t vertical[IMAGE_HEIGHT];
- int32_t frame_time;
+ struct timeval frame_time;
int16_t roll;
int16_t pitch;
};