Diff: SensorManager.cpp

Revision:

0:b6301e4d05a5

diff -r 000000000000 -r b6301e4d05a5 SensorManager.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorManager.cpp	Tue Feb 13 05:22:55 2018 +0000
@@ -0,0 +1,71 @@
+#include "SensorManager.h"
+
+SensorMgr::SensorMgr(PinName mosi,PinName miso,PinName sck,PinName acc_cs,PinName gyr_cs,PinName mag_cs,PinName bmp_cs,PinName gmosi,PinName gmiso,PinName gsck,PinName gcs):bmx(mosi,miso,sck,acc_cs,gyr_cs,mag_cs),bmp(mosi,miso,sck,bmp_cs),gps(gmosi,gmiso,gsck,gcs)
+{
+//センサー類初期化
+    
+    bmp.init(OSR_1,OSR_4,IIR_0);
+    
+
+}
+
+
+void SensorMgr::UpdateObsvd(obsvdData* obs){
+    
+    //センサ観測値更新
+    if(bmp.update()==1){
+        obs->PHeight=bmp.getpress();
+    }else{
+        obs->PHeight=0;
+    }
+    
+    
+    if(bmx.read_gyr(gyr)){
+        obs->Gyr[0]=(int)gyr[0];
+        obs->Gyr[1]=(int)gyr[1];
+        obs->Gyr[2]=(int)gyr[2];
+    }else{
+        obs->Gyr[0]=0;
+        obs->Gyr[1]=0;
+        obs->Gyr[2]=0;
+    }
+    
+    if(bmx.read_acc(acc)){
+        obs->Acc[0]=(int)acc[0];
+        obs->Acc[1]=(int)acc[1];
+        obs->Acc[2]=(int)acc[2];
+    }else{
+        obs->Acc[0]=0;
+        obs->Acc[1]=0;
+        obs->Acc[2]=0;
+    }
+    
+    if(bmx.read_mag(mag)){
+        obs->Mag[0]=(int)mag[0];
+        obs->Mag[1]=(int)mag[1];
+        obs->Mag[2]=(int)mag[2];
+    }else{
+        obs->Mag[0]=0;
+        obs->Mag[1]=0;
+        obs->Mag[2]=0;
+    }
+    
+
+    
+    if(gps.updateData()){
+        gps.chkData(obs);
+    }else{
+        obs->Lon=0;
+        obs->Lat=0;
+        obs->GHeight=0;
+        obs->VelN=0;
+        obs->VelE=0;
+        obs->VelD=0;
+    }
+
+}
+
+void SensorMgr::UpdateTim(tim* t){
+    gps.updateData();
+    gps.chkTime(t);
+}
\ No newline at end of file