Diff: Offsets.cpp

Revision:

5:c3caf8b83e6b

Child:

9:4e94a16ca90c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Offsets.cpp	Fri Dec 19 13:33:21 2014 +0000
@@ -0,0 +1,103 @@
+/*
+2014_12_15
+Author: Michał Szewc
+Free to use
+*/
+
+#include "Offsets.h"
+
+
+Offsets::Offsets()
+{
+    reset();
+}
+
+Offsets::~Offsets(void)
+{
+}
+
+void Offsets::reset()
+{
+    i=0;
+}
+
+
+void Offsets::setOffsets(float *o, Serial &serial, IMU &imu)
+{
+    float offsetGyr[3];
+    char znak;
+    char buff[120];
+    int i;
+    float d[9];
+    
+    sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press o\n\r");
+    serial.printf(buff);
+    do{
+        if(serial.readable()){
+        znak=serial.getc();
+        }
+        sprintf(buff, "Still waiting for o ...\n\r");
+        serial.printf(buff);
+        wait(3);
+    }while(znak!='o');
+    znak=0;
+    sprintf(buff, "Thanks! Gyroscope offsets are being set\n\r");
+    serial.printf(buff);
+    for (i=0;i<1000;i++){
+        imu.readData(d);
+        offsetGyr[0]+=d[0];
+        offsetGyr[1]+=d[1];
+        offsetGyr[2]+=d[2];
+        }
+        offsetGyr[0]/=1000;
+        offsetGyr[1]/=1000;
+        offsetGyr[2]/=1000;
+        
+        //o[0] = offsetGyr[0];
+        //o[1] = offsetGyr[1];
+        //o[2] = offsetGyr[2];
+        // lub tak:
+        *(o+0) = offsetGyr[0];
+        *(o+1) = offsetGyr[1];
+        *(o+2) = offsetGyr[2];
+        
+    sprintf(buff, "Gyroscope offsets in DPS:\n\r");
+    serial.printf(buff);
+    sprintf(buff, "offsetGyro[0]: %f,offsetGyro[1]: %f,offsetGyro[2]: %f,\n\r",(offsetGyr[0]*180/M_PI),(offsetGyr[1]*180/M_PI),(offsetGyr[2]*180/M_PI));
+    serial.printf(buff);
+    sprintf(buff, "Press e to accept\n\r");
+    serial.printf(buff);
+     do{
+        if(serial.readable()){
+        znak=serial.getc();
+        }
+        sprintf(buff, "Press e ...\n\r");
+        serial.printf(buff);
+        wait(3); 
+    }while(znak!='e');
+    znak=0;
+    return;
+    
+    
+}
+
+
+void Offsets::offsetData(float *d, float *D, float *O) 
+{
+    /*for (int i=0; i<3; i++) {
+         O[i] = d[i]-D[i]; 
+    }*/
+    O[0] = d[0]-D[0];
+    O[1] = d[1]-D[1];
+    O[2] = d[2]-D[2];
+}
+
+void Offsets::offsetData2(double *d, float *D, float *O) 
+{
+    /*for (int i=0; i<3; i++) {
+         O[i] = d[i]-D[i]; 
+    }*/
+    O[0] = d[0]-D[0];
+    O[1] = d[1]-D[1];
+    O[2] = d[2]-D[2];
+}
\ No newline at end of file