quadramp issues -- not really fixed

diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c
index 793df389b8b6f19229793832d9e4c3d9e01bf4c4..26d80c5660a5860ca71c9d2a614428e004933bec 100644 (file)
--- a/modules/devices/control_system/filters/quadramp/quadramp.c
+++ b/modules/devices/control_system/filters/quadramp/quadramp.c
@@ -67,7 +67,7 @@ void quadramp_set_1st_order_vars(struct quadramp_filter *q,
 
 uint8_t quadramp_is_finished(struct quadramp_filter *q)
 {
-       return (q->previous_out == q->previous_in &&
+       return ((int32_t)q->previous_out == q->previous_in &&
                q->previous_var == 0);
 }
 
@@ -88,13 +88,22 @@ int32_t quadramp_do_filter(void * data, int32_t in)
        double var_2nd_ord_pos = q->var_2nd_ord_pos;
        double var_2nd_ord_neg = -q->var_2nd_ord_neg;
        double previous_var, d_float;
-       int32_t previous_out;
+       double previous_out;
 
        previous_var = q->previous_var;
        previous_out = q->previous_out;
 
-       d = in - previous_out ;
-       d_float = d;
+       d_float = (double)in - previous_out ;
+
+       /* d is very small, we can jump to dest */
+       if (fabs(d_float) < 2.) {
+               q->previous_var = 0;
+               q->previous_out = in;
+               q->previous_in = in;
+               return in;
+       }
+
+       d = (int32_t)d_float;
 
        /* Deceleration ramp */
        if (d > 0 && var_2nd_ord_neg) {
@@ -173,7 +182,7 @@ int32_t quadramp_do_filter(void * data, int32_t in)
                previous_var = var_1st_ord_neg ;
        }
        else {
-               pos_target = previous_out + d ;
+               pos_target = in;
                previous_var = d_float ;
        }
 

diff --git a/modules/devices/control_system/filters/quadramp/quadramp.h b/modules/devices/control_system/filters/quadramp/quadramp.h
index 7073df03517d880becd3203a3f8f08299d1781cd..c88f08a31bbf357f4428e5d228b3966473edf25c 100644 (file)
--- a/modules/devices/control_system/filters/quadramp/quadramp.h
+++ b/modules/devices/control_system/filters/quadramp/quadramp.h
@@ -32,7 +32,7 @@ struct quadramp_filter
        double var_1st_ord_neg;
 
        double previous_var;
-       int32_t previous_out;
+       double previous_out;
        int32_t previous_in;
 };