From: zer0 <zer0@carbon.local>
Date: Wed, 5 May 2010 18:42:17 +0000 (+0200)
Subject: quadramp issues -- not really fixed
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=3b0f95e3cbbeeefef10dfba0566ef940c54b940e

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 793df38..26d80c5 100644
--- 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 7073df0..c88f08a 100644
--- 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;
 };