Correction of NaN bugs and switch to a median filter instead of an average filter

Neopibox · Neopibox · commit 952eb52daa53 · 2018-04-17T22:34:16.000+02:00
diff --git a/code/ViveBeacons/ViveBeacons.cydsn/Position_finder.c b/code/ViveBeacons/ViveBeacons.cydsn/Position_finder.c
@@ -43,6 +43,11 @@ void Position_finder_init(Position_finder *position_finder, Position3D* beacon_p
 
 void Position_finder_find_position(Position_finder *position_finder, VIVE_sensors_data* vive_sensors_data) {
     Position2D* led_positions[8];
+    double *heading_values = NULL;
+    
+    for(int i = 0; i < 8; i++) {
+        led_positions[i] = NULL;
+    }
 
     // Update data
     if(vive_sensors_data->axis == VERTICAL_AXIS) {
@@ -59,7 +64,7 @@ void Position_finder_find_position(Position_finder *position_finder, VIVE_sensor
     
     // Process LED angles
     for(int i = 0; i < 8; i++) {
-        if(position_finder->vive_sensors_data_h->angles[i] == ANGLE_invalid_value || position_finder->vive_sensors_data_v->angles[i] == ANGLE_invalid_value)
+        if(isnan(position_finder->vive_sensors_data_h->angles[i]) || isnan(position_finder->vive_sensors_data_v->angles[i]))
             continue;
         
         double v_angle = position_finder->beacon_position->alpha+position_finder->vive_sensors_data_v->angles[i];
@@ -76,11 +81,14 @@ void Position_finder_find_position(Position_finder *position_finder, VIVE_sensor
     double sum = 0;
     int nb_couple = 0;
     
-    for(int i = 0; i < 8; i++) {
-        for(int j = i+1; j < 8; j++) {
+    for(int i = 0; i < 8; i+=2) {
+        for(int j = i+2; j < 8; j+=2) {
             if(led_positions[i] == NULL || led_positions[j] == NULL)
                 continue;
             
+            if(isnan(led_positions[i]->x) || isnan(led_positions[i]->y) || isnan(led_positions[j]->x) || isnan(led_positions[j]->y))
+                continue; 
+            
             double applied_delta_x = led_positions[j]->x - led_positions[i]->x;
             double applied_delta_y = led_positions[j]->y - led_positions[i]->y;
             double applied_angle = atan2(applied_delta_y, applied_delta_x);
@@ -89,8 +97,9 @@ void Position_finder_find_position(Position_finder *position_finder, VIVE_sensor
             double theorical_delta_y = position_finder->tracker_led_offset[j][Y_AXIS] - position_finder->tracker_led_offset[i][Y_AXIS];           
             double theorical_angle = atan2(theorical_delta_y, theorical_delta_x);
             
-            sum += Position_finder_normalize_angle(applied_angle - theorical_angle);
             nb_couple++;
+            heading_values = (double*) realloc(heading_values, nb_couple*sizeof(double));
+            heading_values[nb_couple-1] = Position_finder_normalize_angle(applied_angle - theorical_angle);
         }
     }
     
@@ -100,7 +109,19 @@ void Position_finder_find_position(Position_finder *position_finder, VIVE_sensor
         free(current_led_position);
     }
     
-    position_finder->current_position.a = sum / nb_couple;
+    if(nb_couple > 1) {
+        // -- Median filter --
+        qsort(heading_values, nb_couple, sizeof(double), Position_finder_compare); // sort
+        
+        if((nb_couple % 2) == 0) // If length of heading_values is even
+            position_finder->current_position.a = (heading_values[(int) floor(nb_couple/2.0)] + heading_values[(int) ceil(nb_couple/2.0)]) / 2;
+        else
+            position_finder->current_position.a = heading_values[nb_couple/2];
+    } else if(nb_couple == 1) {
+        position_finder->current_position.a = heading_values[0];
+    }
+    else
+        position_finder->current_position.a = NAN;
 }
 
 double Position_finder_normalize_angle(double angle) {
@@ -112,4 +133,21 @@ double Position_finder_normalize_angle(double angle) {
     
     return angle;
 }
+
+static int Position_finder_compare(void const *a, void const *b)
+{
+    int ret = 0;
+    double const *pa = a;
+    double const *pb = b;
+    double diff = *pa - *pb;
+
+    if (diff > 0)
+        ret = 1;
+    else if (diff < 0)
+        ret = -1;
+    else
+        ret = 0;
+
+    return ret;
+}
 /* [] END OF FILE */
diff --git a/code/ViveBeacons/ViveBeacons.cydsn/Position_finder.h b/code/ViveBeacons/ViveBeacons.cydsn/Position_finder.h
@@ -38,7 +38,7 @@ void Position_finder_find_position(Position_finder *position_finder, VIVE_sensor
 
 // Utils
 double Position_finder_normalize_angle(double angle);
-
+static int Position_finder_compare(void const *a, void const *b); // For median filter
 
 #endif
 
