ロケットロガーのセンサ関連を管理するプログラム
Dependencies: BMP280 BMX055 ublox_UBX
Fork of SensorManager by
SensorManager.cpp@0:b6301e4d05a5, 2018-02-13 (annotated)
- Committer:
- kim1212
- Date:
- Tue Feb 13 05:22:55 2018 +0000
- Revision:
- 0:b6301e4d05a5
???
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kim1212 | 0:b6301e4d05a5 | 1 | #include "SensorManager.h" |
kim1212 | 0:b6301e4d05a5 | 2 | |
kim1212 | 0:b6301e4d05a5 | 3 | 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) |
kim1212 | 0:b6301e4d05a5 | 4 | { |
kim1212 | 0:b6301e4d05a5 | 5 | //センサー類初期化 |
kim1212 | 0:b6301e4d05a5 | 6 | |
kim1212 | 0:b6301e4d05a5 | 7 | bmp.init(OSR_1,OSR_4,IIR_0); |
kim1212 | 0:b6301e4d05a5 | 8 | |
kim1212 | 0:b6301e4d05a5 | 9 | |
kim1212 | 0:b6301e4d05a5 | 10 | } |
kim1212 | 0:b6301e4d05a5 | 11 | |
kim1212 | 0:b6301e4d05a5 | 12 | |
kim1212 | 0:b6301e4d05a5 | 13 | void SensorMgr::UpdateObsvd(obsvdData* obs){ |
kim1212 | 0:b6301e4d05a5 | 14 | |
kim1212 | 0:b6301e4d05a5 | 15 | //センサ観測値更新 |
kim1212 | 0:b6301e4d05a5 | 16 | if(bmp.update()==1){ |
kim1212 | 0:b6301e4d05a5 | 17 | obs->PHeight=bmp.getpress(); |
kim1212 | 0:b6301e4d05a5 | 18 | }else{ |
kim1212 | 0:b6301e4d05a5 | 19 | obs->PHeight=0; |
kim1212 | 0:b6301e4d05a5 | 20 | } |
kim1212 | 0:b6301e4d05a5 | 21 | |
kim1212 | 0:b6301e4d05a5 | 22 | |
kim1212 | 0:b6301e4d05a5 | 23 | if(bmx.read_gyr(gyr)){ |
kim1212 | 0:b6301e4d05a5 | 24 | obs->Gyr[0]=(int)gyr[0]; |
kim1212 | 0:b6301e4d05a5 | 25 | obs->Gyr[1]=(int)gyr[1]; |
kim1212 | 0:b6301e4d05a5 | 26 | obs->Gyr[2]=(int)gyr[2]; |
kim1212 | 0:b6301e4d05a5 | 27 | }else{ |
kim1212 | 0:b6301e4d05a5 | 28 | obs->Gyr[0]=0; |
kim1212 | 0:b6301e4d05a5 | 29 | obs->Gyr[1]=0; |
kim1212 | 0:b6301e4d05a5 | 30 | obs->Gyr[2]=0; |
kim1212 | 0:b6301e4d05a5 | 31 | } |
kim1212 | 0:b6301e4d05a5 | 32 | |
kim1212 | 0:b6301e4d05a5 | 33 | if(bmx.read_acc(acc)){ |
kim1212 | 0:b6301e4d05a5 | 34 | obs->Acc[0]=(int)acc[0]; |
kim1212 | 0:b6301e4d05a5 | 35 | obs->Acc[1]=(int)acc[1]; |
kim1212 | 0:b6301e4d05a5 | 36 | obs->Acc[2]=(int)acc[2]; |
kim1212 | 0:b6301e4d05a5 | 37 | }else{ |
kim1212 | 0:b6301e4d05a5 | 38 | obs->Acc[0]=0; |
kim1212 | 0:b6301e4d05a5 | 39 | obs->Acc[1]=0; |
kim1212 | 0:b6301e4d05a5 | 40 | obs->Acc[2]=0; |
kim1212 | 0:b6301e4d05a5 | 41 | } |
kim1212 | 0:b6301e4d05a5 | 42 | |
kim1212 | 0:b6301e4d05a5 | 43 | if(bmx.read_mag(mag)){ |
kim1212 | 0:b6301e4d05a5 | 44 | obs->Mag[0]=(int)mag[0]; |
kim1212 | 0:b6301e4d05a5 | 45 | obs->Mag[1]=(int)mag[1]; |
kim1212 | 0:b6301e4d05a5 | 46 | obs->Mag[2]=(int)mag[2]; |
kim1212 | 0:b6301e4d05a5 | 47 | }else{ |
kim1212 | 0:b6301e4d05a5 | 48 | obs->Mag[0]=0; |
kim1212 | 0:b6301e4d05a5 | 49 | obs->Mag[1]=0; |
kim1212 | 0:b6301e4d05a5 | 50 | obs->Mag[2]=0; |
kim1212 | 0:b6301e4d05a5 | 51 | } |
kim1212 | 0:b6301e4d05a5 | 52 | |
kim1212 | 0:b6301e4d05a5 | 53 | |
kim1212 | 0:b6301e4d05a5 | 54 | |
kim1212 | 0:b6301e4d05a5 | 55 | if(gps.updateData()){ |
kim1212 | 0:b6301e4d05a5 | 56 | gps.chkData(obs); |
kim1212 | 0:b6301e4d05a5 | 57 | }else{ |
kim1212 | 0:b6301e4d05a5 | 58 | obs->Lon=0; |
kim1212 | 0:b6301e4d05a5 | 59 | obs->Lat=0; |
kim1212 | 0:b6301e4d05a5 | 60 | obs->GHeight=0; |
kim1212 | 0:b6301e4d05a5 | 61 | obs->VelN=0; |
kim1212 | 0:b6301e4d05a5 | 62 | obs->VelE=0; |
kim1212 | 0:b6301e4d05a5 | 63 | obs->VelD=0; |
kim1212 | 0:b6301e4d05a5 | 64 | } |
kim1212 | 0:b6301e4d05a5 | 65 | |
kim1212 | 0:b6301e4d05a5 | 66 | } |
kim1212 | 0:b6301e4d05a5 | 67 | |
kim1212 | 0:b6301e4d05a5 | 68 | void SensorMgr::UpdateTim(tim* t){ |
kim1212 | 0:b6301e4d05a5 | 69 | gps.updateData(); |
kim1212 | 0:b6301e4d05a5 | 70 | gps.chkTime(t); |
kim1212 | 0:b6301e4d05a5 | 71 | } |