Diff: setting.cpp

Revision:

0:2299c333a9b4

Child:

1:b2263c93d1d4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/setting.cpp	Wed Jan 20 15:48:38 2016 +0000
@@ -0,0 +1,225 @@
+#include "setting.h"
+
+void setting::setGpsPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
+    
+    if(changeFlag != 0){
+        pastGpsPoint.x = gpsPoint.x;
+        pastGpsPoint.y = gpsPoint.y;
+        
+        gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
+        gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
+        
+        setGpsDistance();
+ //       flag++;
+    }
+    else{
+//        flag = 1;
+        gpsPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
+        gpsPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
+        changeFlag = 1;
+    }
+   
+    
+}
+
+void setting::setGpsDistance(void){
+    
+    gpsDis.x = (gpsPoint.x - pastGpsPoint.x) * 111120;
+    gpsDis.y = (gpsPoint.y - pastGpsPoint.y) * dis * 1000;
+    
+}
+
+void setting::setAngle(double angle){
+    
+    if(changeFlag != 0){
+        pastGpsAngle = gpsAngle;
+        gpsAngle = angle;
+        difAngle = gpsAngle - pastGpsAngle;
+    }
+    else{
+        gpsAngle = angle;
+    }
+    
+}
+
+void setting::setCmp(int16_t x, int16_t y, int16_t z){
+    
+/*        agzCmp.x = x;
+        agzCmp.y = y;
+        agzCmp.z = z;
+
+        if(flag == 1){
+            minCmp.x = x; maxCmp.x = x;
+            minCmp.y = y; maxCmp.y = y;
+            minCmp.z = z; maxCmp.z = z;
+        }
+        else{
+            if(minCmp.x > x){
+                minCmp.x = x;
+            }
+            else if(maxCmp.x < x){
+                maxCmp.x = x;
+            }
+            if(minCmp.y > y){
+                minCmp.y = y;
+            }
+            else if(maxCmp.y < y){
+                maxCmp.y = y;
+            }
+            if(minCmp.z > z){
+                minCmp.z = z;
+            }
+            else if(maxCmp.z < z){
+                maxCmp.z = z;
+            }
+        }
+
+*/
+    
+   if(flag <= 20){
+        caribCmp.x += x;
+        caribCmp.y += y;
+        caribCmp.z += z;
+        
+        agzCmp.x = x;
+        agzCmp.y = y;
+        agzCmp.z = z;
+    }
+    
+    else{
+        
+        agzCmp.x = x - caribCmp.x;
+        agzCmp.y = y - caribCmp.y;
+        agzCmp.z = z - caribCmp.z;
+  
+    }
+    
+    if(flag == 20){
+        caribCmp.x /= 20;
+        caribCmp.y /= 20;
+        caribCmp.z /= 20;
+   } 
+    
+    
+}
+
+void setting::setAcc(int16_t x, int16_t y, int16_t z){
+    
+/*    agzAcc.x = x;
+    agzAcc.y = y;
+    agzAcc.z = z;
+
+    if(flag == 1){
+        minAcc.x = x; maxAcc.x = x;
+        minAcc.y = y; maxAcc.y = y;
+        minAcc.z = z; maxAcc.z = z;
+    }
+    else{
+        if(minAcc.x > x){
+            minAcc.x = x;
+        }
+        else if(maxAcc.x < x){
+            maxAcc.x = x;
+        }
+        if(minCmp.y > y){
+            minAcc.y = y;
+        }
+        else if(maxAcc.y < y){
+            maxAcc.y = y;
+        }
+        if(minAcc.z > z){
+            minAcc.z = z;
+        }
+        else if(maxAcc.z < z){
+            maxAcc.z = z;
+        }
+    }
+*/
+    if(flag <= 20){
+        caribAcc.x += x;
+        caribAcc.y += y;
+        caribAcc.z += z;
+        
+        agzAcc.x = x;
+        agzAcc.y = y;
+        agzAcc.z = z;
+    }
+    
+    else{
+        
+        agzAcc.x = x - caribAcc.x;
+        agzAcc.y = y - caribAcc.y;
+        agzAcc.z = z - caribAcc.z;
+        
+    }
+    
+    if(flag == 20){
+       caribAcc.x /= 20;
+        caribAcc.y /= 20;
+        caribAcc.z /= 20;
+
+   } 
+    
+}
+
+void setting::setGyr(int16_t x, int16_t y, int16_t z){
+    
+/*    agzGyr.x = x;
+    agzGyr.y = y;
+    agzGyr.z = z;
+
+    if(flag == 1){
+        minGyr.x = x; maxGyr.x = x;
+        minGyr.y = y; maxGyr.y = y;
+        minGyr.z = z; maxGyr.z = z;
+    }
+    else{
+        if(minGyr.x > x){
+            minGyr.x = x;
+        }
+        else if(maxGyr.x < x){
+            maxGyr.x = x;
+        }
+        if(minGyr.y > y){
+            minGyr.y = y;
+        }
+        else if(maxGyr.y < y){
+            maxGyr.y = y;
+        }
+        if(minGyr.z > z){
+            minGyr.z = z;
+        }
+        else if(maxGyr.z < z){
+            maxGyr.z = z;
+        }
+    }
+*/
+    if(flag <= 20){
+        caribGyr.x += x;
+        caribGyr.y += y;
+        caribGyr.z += z;
+        
+        agzGyr.x = x;
+        agzGyr.y = y;
+        agzGyr.z = z;
+    }
+    
+    else{
+        
+       pastGyr.x = agzGyr.x;
+        pastGyr.y = agzGyr.y;
+        pastGyr.z = agzGyr.z;
+        
+        agzGyr.x = (x - caribGyr.x);
+        agzGyr.y = (y - caribGyr.y);
+        agzGyr.z = (z - caribGyr.z);
+        
+    }
+    
+    if(flag == 20){
+       caribGyr.x /= 20;
+        caribGyr.y /= 20;
+        caribGyr.z /= 20;
+   } 
+  
+}