robofsm: Median filter in orte callback for sharps

not tested

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 559af5b0087686651589cc2bf7308d71fe6d310a..0578374cbde6dc7b89c57724b67ab4e8878297bc 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -302,15 +302,39 @@ void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-#define AVG_CNT 10
+#define HIST_CNT 5
+
+static int cmp_double(void *v1, void *v2)
+{
+       double *d1 = v1, *d2 = v2;
+       if (d1 < d2)
+               return -1;
+       else if (d1 > d2)
+               return +1;
+       else
+               return 0;
+}
+
+static inline double *sharp_ptr(struct sharps_type *sharps, int index)
+{
+       switch (index) {
+               case 0: return &sharps->front_left; break;
+               case 1: return &sharps->front_right; break;
+               case 2: return &sharps->left; break;
+               case 3: return &sharps->right; break;
+               default: return &sharps->front_left; break;
+       }
+
+}
 
 void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
        struct sharps_type *instance = (struct sharps_type *)vinstance;
-       static struct sharps_type history[AVG_CNT];
+       static struct sharps_type history[HIST_CNT];
+       static double history_sorted[HIST_CNT];
        static int ind = -1;
-       int i;
+       int i, s;
        //struct sharps_type *for_mcl;
 
        switch (info->status) {
@@ -319,7 +343,7 @@ void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
                        robot.sharps = *instance;
                        ROBOT_UNLOCK(sharps);
                        if (ind == -1) {
-                               for (i=0; i<AVG_CNT; i++) {
+                               for (i=0; i<HIST_CNT; i++) {
                                        history[i].front_left = 0.8;
                                        history[i].front_right = 0.8;
                                        history[i].left = 0.8;
@@ -327,17 +351,18 @@ void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
                                }
                        }
                        ind++;
-                       if (ind >= AVG_CNT) {
+                       if (ind >= HIST_CNT) {
                                ind = 0;
                        }
                        history[ind] = *instance;
-                       memset(instance, 0, sizeof(*instance));
-                       for (i=0; i<AVG_CNT; i++) {
-                               instance->front_left += history[i].front_left / AVG_CNT;
-                               instance->front_right += history[i].front_right / AVG_CNT;
-                               instance->left += history[i].left / AVG_CNT;
-                               instance->right += history[i].right / AVG_CNT;
+
+                       for (s=0; s<4; s++) {
+                               for (i=0; i<HIST_CNT; i++) {
+                                       history_sorted[i] = *sharp_ptr(&history[i], s);
+                               }
+                               qsort(history_sorted, HIST_CNT, sizeof(history_sorted[0]), cmp_double);
                        }
+                       *sharp_ptr(instance, s) = history_sorted[HIST_CNT/2]; /* Get median */
 
                        update_map(instance);
                        break;