ringbufs, data fusion

2 files changed, 36 insertions, 18 deletions

diff --git a/client2.cpp b/client2.cpp
index e09dbfc..826b789 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -99,6 +99,34 @@ int main(int argc, const char** argv)
 		total_y += shift_y;
 		total_rot = fixup_angle(total_rot+angle);
 
+		ringbuf_x.put(total_x);
+		ringbuf_y.put(total_y);
+		ringbuf_a.put(total_rot);
+		ringbuf_phi.put(navdata.phi);
+		ringbuf_psi.put(navdata.psi);
+		ringbuf_theta.put(navdata.theta);
+
+		double xdiff = fixup_range( ringbuf_x.get() - px_per_deg*ringbuf_psi.get(), -virtual_canvas_width/2, virtual_canvas_width/2);
+		double ydiff = ringbuf_y.get() + px_per_deg*ringbuf_theta.get();
+		double adiff = fixup_angle(ringbuf_a.get() - (-ringbuf_phi.get()));
+		
+		//if (fabs(xdiff) < px_per_deg) xdiff = 0.0;
+		//if (fabs(ydiff) < px_per_deg) ydiff = 0.0;
+		//if (fabs(adiff) < 2) adiff = 0.0;
+
+		xdiff*=0.3;
+		ydiff*=0.3;
+		adiff*=0.3;
+		total_x = fixup_range(total_x - xdiff, -virtual_canvas_width/2, virtual_canvas_width/2);
+		total_y = total_y - ydiff;
+		total_rot = fixup_angle(total_rot - adiff);
+		ringbuf_x.add(-xdiff);
+		ringbuf_y.add(-ydiff);
+		ringbuf_a.add(-adiff);
+
+
+
+
 		printf("sh:  %i\t%i\t%f\n", shift_x, shift_y, angle);
 		printf("tot: %i\t%i\t%f\n", total_x, total_y, total_rot);
 
@@ -121,24 +149,6 @@ int main(int argc, const char** argv)
 		oldgray = gray.clone();
 
 
-		ringbuf_x.put(total_x);
-		ringbuf_y.put(total_y);
-		ringbuf_a.put(total_rot);
-		ringbuf_phi.put(navdata.phi);
-		ringbuf_psi.put(navdata.psi);
-		ringbuf_theta.put(navdata.theta);
-
-		double xdiff = fixup_range( ringbuf_x.get() - px_per_deg*ringbuf_psi.get(), -virtual_canvas_width/2, virtual_canvas_width/2);
-		double ydiff = ringbuf_y.get() + px_per_deg*ringbuf_theta.get();
-		double adiff = fixup_angle(ringbuf_a.get() - (-ringbuf_phi.get()));
-
-		total_x = fixup_range(total_x - xdiff/20.0, -virtual_canvas_width/2, virtual_canvas_width/2);
-		total_y = total_y - ydiff/20.0;
-		total_rot = fixup_angle(total_rot - adiff/20.0);
-
-
-
-
 	}
 	
 	return 0;
diff --git a/ringbuf.h b/ringbuf.h
index 5756153..1f97a12 100644
--- a/ringbuf.h
+++ b/ringbuf.h
@@ -123,4 +123,12 @@ class ModuloRingbuffer
 	{
 		rb->set(_fixup_range(val));
 	}
+
+	void add(double val)
+	{
+		rb->add(val);
+		
+		while (get() < low) rb->add(span);
+		while (get()>= upp) rb->add(-span);
+	}
 };