summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--client2.cpp30
-rw-r--r--ringbuf.h126
2 files changed, 156 insertions, 0 deletions
diff --git a/client2.cpp b/client2.cpp
index 8f68fbc..e09dbfc 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -8,6 +8,7 @@
#include <opencv2/opencv.hpp>
#include "lib.h"
+#include "ringbuf.h"
#define PI 3.141593654
@@ -54,6 +55,15 @@ int main(int argc, const char** argv)
Mat frame, gray, oldgray;
Mat screencontent(real_canvas_height,real_canvas_width, CV_32FC3);
+ #define RINGBUF_SIZE 10
+ ModuloRingbuffer ringbuf_x(RINGBUF_SIZE, -virtual_canvas_width/2, virtual_canvas_width/2);
+ Ringbuffer ringbuf_y(RINGBUF_SIZE);
+ ModuloRingbuffer ringbuf_a(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_phi(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_psi(RINGBUF_SIZE, -180,180);
+ ModuloRingbuffer ringbuf_theta(RINGBUF_SIZE, -180,180);
+
+
for (int i=0; i<160;i++)
{
drone.get(frame, &navdata);
@@ -109,6 +119,26 @@ int main(int argc, const char** argv)
imshow("screencontent", screencontent);
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
new file mode 100644
index 0000000..5756153
--- /dev/null
+++ b/ringbuf.h
@@ -0,0 +1,126 @@
+#include <math.h>
+
+class Ringbuffer
+{
+ public:
+
+ Ringbuffer(int size)
+ {
+ this->size = size;
+ idx = 0;
+ buf = new double[size];
+ for (int i=0; i<size; i++)
+ buf[i] = 0;
+ avg = 0.0;
+ avg_valid = false;
+ }
+
+ ~Ringbuffer()
+ {
+ delete [] buf;
+ }
+
+ double get()
+ {
+ if (!avg_valid)
+ {
+ avg=0.0;
+ for (int i=0; i<size; i++)
+ avg += buf[i];
+ avg/=size;
+ avg_valid=true;
+ }
+
+ return avg;
+ }
+
+ void put(double val)
+ {
+ buf[idx] = val;
+ idx = (idx+1) % size;
+ avg_valid = false;
+ }
+
+ void set(double val)
+ {
+ for (int i=0; i<size; i++)
+ buf[i]=val;
+ avg = val;
+ avg_valid=true;
+ }
+
+ void add(double val)
+ {
+ for (int i=0; i<size; i++)
+ buf[i]+=val;
+ avg += val;
+ }
+
+ private:
+ double* buf;
+ int idx;
+ int size;
+ double avg;
+ bool avg_valid;
+};
+
+class ModuloRingbuffer
+{
+ private:
+ Ringbuffer* rb;
+ double low,upp,span;
+
+ public:
+
+ ModuloRingbuffer(int size, double low, double upp)
+ {
+ rb = new Ringbuffer(size);
+ this->low=low;
+ this->upp=upp;
+ this->span=upp-low;
+ rb->set(_fixup_range(0.0));
+ }
+
+ ~ModuloRingbuffer()
+ {
+ delete rb;
+ }
+
+ double _fixup_range(double val)
+ {
+ while (val < low) val+=span;
+ while (val>= upp) val-=span;
+ return val;
+ }
+
+ double get()
+ {
+ return rb->get();
+ }
+
+ void put(double val)
+ {
+ val=_fixup_range(val);
+
+ // direct way
+ double dist1 = val - get();
+ // over borders
+ double dist2 = dist1 + span;
+ double dist3 = dist1 - span;
+
+ if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+ rb->put(val);
+ else if (fabs(dist2) <= fabs(dist3))
+ rb->put(val+span);
+ else
+ rb->put(val-span);
+
+ while (get() < low) rb->add(span);
+ while (get()>= upp) rb->add(-span);
+ }
+
+ void set(double val)
+ {
+ rb->set(_fixup_range(val));
+ }
+};