diff options
| -rw-r--r-- | client2.cpp | 30 | ||||
| -rw-r--r-- | ringbuf.h | 126 |
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)); + } +}; |