FTE-denshi / Mbed 2 deprecated Izu_2019

2019伊豆のコード

Dependencies:   mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis

main.cpp@8:b358cd848797, 2020-09-25 (annotated)

Committer:
zeutel
Date:
Fri Sep 25 14:33:10 2020 +0000
Revision:
8:b358cd848797
Parent:
7:1f2508ade0a3 
commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zeutel 0:cebf1f73ffd5 1 #include "mbed.h"
zeutel 0:cebf1f73ffd5 2 #include "LPS331_I2C.h"
zeutel 0:cebf1f73ffd5 3 #include "LSM9DS1.h"
zeutel 0:cebf1f73ffd5 4 #include "SDFileSystem.h"
zeutel 0:cebf1f73ffd5 5 #include "IM920.h"
Nozaryo 3:1f93c4510fb1 6 #include "millis.h"
zeutel 7:1f2508ade0a3 7 #include "ADXL345_I2C.h"
zeutel 0:cebf1f73ffd5 8 #include "GPS.h"
zeutel 7:1f2508ade0a3 9 #include <stdio.h>
zeutel 7:1f2508ade0a3 10 #include <string.h>
zeutel 0:cebf1f73ffd5 11
zeutel 7:1f2508ade0a3 12 //ピンとか
zeutel 7:1f2508ade0a3 13 Serial pc(USBTX, USBRX);
zeutel 7:1f2508ade0a3 14 DigitalIn flightpin(p12);//フライトピン
zeutel 7:1f2508ade0a3 15 LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);//気温気圧
zeutel 7:1f2508ade0a3 16 ADXL345_I2C accelerometer(p9, p10);//オープニングショック
zeutel 7:1f2508ade0a3 17 LSM9DS1 lsm(p9,p10);//9軸
zeutel 7:1f2508ade0a3 18 IM920 im920(p28, p27, p29, p30);//無線
zeutel 7:1f2508ade0a3 19 GPS gps(p13, p14);//GPS
zeutel 7:1f2508ade0a3 20 SDFileSystem sd(p5, p6, p7, p8, "sd");//SD
zeutel 7:1f2508ade0a3 21 PwmOut servo_1(p26), servo_2(p25);//サーボ
Nozaryo 3:1f93c4510fb1 22 Timer waitTime;
zeutel 7:1f2508ade0a3 23
zeutel 5:03d31f0a4943 24
zeutel 7:1f2508ade0a3 25
zeutel 7:1f2508ade0a3 26 //function
zeutel 0:cebf1f73ffd5 27 void init(FILE **file);//諸々の初期化
zeutel 0:cebf1f73ffd5 28 void checkingSensors();
zeutel 7:1f2508ade0a3 29 void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings);//センサの読み取り
zeutel 7:1f2508ade0a3 30 void imRecv(char *receive);//無線受信
zeutel 0:cebf1f73ffd5 31 float calcAltitude(float pres, float temp);//高度計算
zeutel 7:1f2508ade0a3 32 void imSend(char *send);
zeutel 7:1f2508ade0a3 33 void mbedOperate(char *command);
zeutel 7:1f2508ade0a3 34 void sendDatas(float time,float temp,float pres,float ax,float ay,float az,float gx,float gy,float gz);//unity用data送信
zeutel 7:1f2508ade0a3 35
zeutel 7:1f2508ade0a3 36 //global
zeutel 7:1f2508ade0a3 37 char sendData[256];
zeutel 7:1f2508ade0a3 38 FILE *fp;
zeutel 7:1f2508ade0a3 39 bool SDisAvailable=false;
zeutel 0:cebf1f73ffd5 40
zeutel 0:cebf1f73ffd5 41 int main() {
zeutel 0:cebf1f73ffd5 42 int sequence=0;//機体の状態の推移
zeutel 5:03d31f0a4943 43 int pretime=0,pretime_2=0;
zeutel 7:1f2508ade0a3 44 float altitude=-1000.0,maxAltitude=-1000.0;//m単位
zeutel 0:cebf1f73ffd5 45 float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
zeutel 0:cebf1f73ffd5 46 char receive[9]={};//受信した文字列
zeutel 0:cebf1f73ffd5 47 bool flightpinAttached=false;//フライトピンが付いているかどうか
zeutel 7:1f2508ade0a3 48 float temperature,pressure;
zeutel 7:1f2508ade0a3 49 float accelReadings[3] = {0, 0, 0};//x,y,z
zeutel 7:1f2508ade0a3 50
Nozaryo 3:1f93c4510fb1 51 millisStart();
zeutel 7:1f2508ade0a3 52
Nozaryo 3:1f93c4510fb1 53 //サーボclose
zeutel 5:03d31f0a4943 54 servo_1.pulsewidth(1.05/1000.0);//ms/1000
Nozaryo 3:1f93c4510fb1 55 servo_2.pulsewidth(1.94/1000.0);
zeutel 0:cebf1f73ffd5 56
zeutel 0:cebf1f73ffd5 57 init(&fp);//初期化
Nozaryo 3:1f93c4510fb1 58 flightpinAttached=flightpin;
zeutel 0:cebf1f73ffd5 59
zeutel 7:1f2508ade0a3 60 while(strncmp(receive,"escape",sizeof(receive))!=0){//準備中
zeutel 7:1f2508ade0a3 61 imRecv(receive);
zeutel 7:1f2508ade0a3 62 mbedOperate(receive);
zeutel 7:1f2508ade0a3 63 readValues(sequence,&temperature,&pressure,accelReadings);
zeutel 7:1f2508ade0a3 64 sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
zeutel 5:03d31f0a4943 65 if(millis()-pretime>5000){
zeutel 5:03d31f0a4943 66 pretime=millis();
zeutel 7:1f2508ade0a3 67 //センサ・フライトピン
zeutel 7:1f2508ade0a3 68 flightpinAttached=flightpin;
zeutel 7:1f2508ade0a3 69 //altitude=calcAltitude(pressure,temperature);
zeutel 7:1f2508ade0a3 70 //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az));
zeutel 7:1f2508ade0a3 71 //imSend(sendData);
zeutel 7:1f2508ade0a3 72 imSend("Waiting: command 'escape' to escape waiting");
zeutel 7:1f2508ade0a3 73 if(flightpinAttached==false){
zeutel 7:1f2508ade0a3 74 imSend("flightpin isn't attached");
zeutel 7:1f2508ade0a3 75 }
zeutel 5:03d31f0a4943 76 }
Nozaryo 3:1f93c4510fb1 77 }
zeutel 7:1f2508ade0a3 78
zeutel 7:1f2508ade0a3 79 imSend("Escape Waiting");
zeutel 7:1f2508ade0a3 80
zeutel 7:1f2508ade0a3 81 while(strncmp(receive,"record",sizeof(receive))!=0){//開始指令待ち
zeutel 7:1f2508ade0a3 82 imRecv(receive);
zeutel 7:1f2508ade0a3 83 mbedOperate(receive);
zeutel 7:1f2508ade0a3 84 //加速度・フライトピン読み取り
zeutel 7:1f2508ade0a3 85 readValues(sequence,&temperature,&pressure,accelReadings);
zeutel 7:1f2508ade0a3 86 sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
Nozaryo 3:1f93c4510fb1 87 flightpinAttached=flightpin;
zeutel 7:1f2508ade0a3 88 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始
zeutel 7:1f2508ade0a3 89 imSend("Forced start");
zeutel 5:03d31f0a4943 90 launchTime=millis();
zeutel 5:03d31f0a4943 91 sequence=1;
zeutel 0:cebf1f73ffd5 92 break;
zeutel 0:cebf1f73ffd5 93 }
Nozaryo 3:1f93c4510fb1 94 //地上局への送信
Nozaryo 3:1f93c4510fb1 95 if(millis() - pretime > 5000){//5秒おきに経過時間送信
zeutel 7:1f2508ade0a3 96 //altitude=calcAltitude(pressure,temperature);
zeutel 7:1f2508ade0a3 97 //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az));
zeutel 7:1f2508ade0a3 98 //imSend(sendData);
zeutel 7:1f2508ade0a3 99 imSend("Start Waiting: command 'record' to start recording");
Nozaryo 3:1f93c4510fb1 100 pretime = millis();
Nozaryo 3:1f93c4510fb1 101 }
zeutel 0:cebf1f73ffd5 102 }
zeutel 7:1f2508ade0a3 103 imSend("RECORDEING START");
zeutel 0:cebf1f73ffd5 104
zeutel 0:cebf1f73ffd5 105 while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了
zeutel 7:1f2508ade0a3 106 imRecv(receive);
zeutel 7:1f2508ade0a3 107 mbedOperate(receive);
zeutel 0:cebf1f73ffd5 108 //センサ・フライトピン読み取り 高度計測
zeutel 7:1f2508ade0a3 109 readValues(sequence,&temperature,&pressure,accelReadings);
zeutel 7:1f2508ade0a3 110 sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
zeutel 0:cebf1f73ffd5 111 flightpinAttached=flightpin;
zeutel 7:1f2508ade0a3 112 altitude=calcAltitude(pressure,temperature);
zeutel 0:cebf1f73ffd5 113
zeutel 0:cebf1f73ffd5 114 if(maxAltitude<altitude){//最高高度更新
zeutel 0:cebf1f73ffd5 115 maxAltitude=altitude;
zeutel 0:cebf1f73ffd5 116 }
zeutel 0:cebf1f73ffd5 117
zeutel 0:cebf1f73ffd5 118 switch(sequence){
zeutel 0:cebf1f73ffd5 119 case 0://発射待機中
zeutel 0:cebf1f73ffd5 120 //離床検知
zeutel 7:1f2508ade0a3 121 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
zeutel 7:1f2508ade0a3 122 imSend("LAUNCH ACC>2.0G");
Nozaryo 3:1f93c4510fb1 123 launchTime=millis();
Nozaryo 3:1f93c4510fb1 124 sequence=1;
Nozaryo 3:1f93c4510fb1 125 }
zeutel 5:03d31f0a4943 126 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=3.9*3.9 &&flightpinAttached==true){//加速度4.0G以上かつフライトピンがぬけてない
zeutel 7:1f2508ade0a3 127 imSend("LAUNCH ACC>3.9G");
Nozaryo 3:1f93c4510fb1 128 launchTime=millis();
zeutel 0:cebf1f73ffd5 129 sequence=1;
zeutel 0:cebf1f73ffd5 130 }
zeutel 0:cebf1f73ffd5 131 break;
zeutel 0:cebf1f73ffd5 132
zeutel 0:cebf1f73ffd5 133 case 1://発射～減速機構展開
zeutel 0:cebf1f73ffd5 134 //減速機構展開
zeutel 7:1f2508ade0a3 135 if(millis()-launchTime>=13*1000){
zeutel 7:1f2508ade0a3 136 if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
zeutel 7:1f2508ade0a3 137 imSend("Start speed reducing");
zeutel 5:03d31f0a4943 138 reducerExpantionedTime=millis();
zeutel 5:03d31f0a4943 139 //サーボopen
zeutel 5:03d31f0a4943 140 servo_1.pulsewidth(1.90/1000);//ms/1000
zeutel 5:03d31f0a4943 141 servo_2.pulsewidth(1.32/1000);
zeutel 5:03d31f0a4943 142 sequence=2;
zeutel 5:03d31f0a4943 143 }
zeutel 0:cebf1f73ffd5 144 }
zeutel 0:cebf1f73ffd5 145 break;
zeutel 0:cebf1f73ffd5 146
zeutel 0:cebf1f73ffd5 147 case 2://減速機構展開～着地
Nozaryo 3:1f93c4510fb1 148 if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
zeutel 7:1f2508ade0a3 149 imSend("LANDING");
zeutel 0:cebf1f73ffd5 150 sequence=3;
zeutel 0:cebf1f73ffd5 151 }
zeutel 0:cebf1f73ffd5 152 break;
zeutel 0:cebf1f73ffd5 153 }
zeutel 0:cebf1f73ffd5 154
zeutel 7:1f2508ade0a3 155 if(millis() - pretime_2 > 2000){//2秒おきに状態送信
zeutel 5:03d31f0a4943 156 sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000);
zeutel 7:1f2508ade0a3 157 imSend(sendData);
zeutel 5:03d31f0a4943 158 pretime_2=millis();
zeutel 5:03d31f0a4943 159 }
zeutel 0:cebf1f73ffd5 160
zeutel 7:1f2508ade0a3 161 if(sequence==1){
zeutel 7:1f2508ade0a3 162 //SD書き込み
zeutel 7:1f2508ade0a3 163 if(SDisAvailable){
zeutel 7:1f2508ade0a3 164 fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
zeutel 7:1f2508ade0a3 165 (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz);
zeutel 7:1f2508ade0a3 166 }
zeutel 7:1f2508ade0a3 167 }else{
zeutel 7:1f2508ade0a3 168 //SD書き込み
zeutel 7:1f2508ade0a3 169 if(SDisAvailable){
zeutel 7:1f2508ade0a3 170 fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
zeutel 7:1f2508ade0a3 171 (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz,gps.longitude,gps.latitude);
zeutel 7:1f2508ade0a3 172 }
zeutel 7:1f2508ade0a3 173
zeutel 7:1f2508ade0a3 174 //地上局へのGPS情報送信
zeutel 7:1f2508ade0a3 175 /*if(millis() - pretime > 5000){//5秒おきにGPS送信
zeutel 7:1f2508ade0a3 176 waitTime.start();
zeutel 7:1f2508ade0a3 177 waitTime.reset();
zeutel 7:1f2508ade0a3 178 while(gps.result==false){//GPSが利用可能になるまでまつ
zeutel 7:1f2508ade0a3 179 gps.getgps();
zeutel 7:1f2508ade0a3 180 if(waitTime.read() > 1){
zeutel 7:1f2508ade0a3 181 waitTime.stop();
zeutel 7:1f2508ade0a3 182 break;
zeutel 7:1f2508ade0a3 183 }
zeutel 7:1f2508ade0a3 184 }
zeutel 7:1f2508ade0a3 185 sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
zeutel 7:1f2508ade0a3 186 imSend(sendData);
zeutel 7:1f2508ade0a3 187 }*/
zeutel 7:1f2508ade0a3 188 pretime=millis();
zeutel 7:1f2508ade0a3 189 }
zeutel 7:1f2508ade0a3 190 }
zeutel 7:1f2508ade0a3 191
zeutel 7:1f2508ade0a3 192 //着地後
zeutel 7:1f2508ade0a3 193 if(SDisAvailable){
zeutel 7:1f2508ade0a3 194 fclose(fp);
zeutel 7:1f2508ade0a3 195 imSend("File closed");
zeutel 7:1f2508ade0a3 196 }else{
zeutel 7:1f2508ade0a3 197 imSend("SD error");
zeutel 7:1f2508ade0a3 198 }
zeutel 7:1f2508ade0a3 199
zeutel 7:1f2508ade0a3 200 sprintf(sendData,"MaxAltitude:%06.2fm",maxAltitude);
zeutel 7:1f2508ade0a3 201 imSend(sendData);
zeutel 7:1f2508ade0a3 202
zeutel 7:1f2508ade0a3 203 while(1){
zeutel 7:1f2508ade0a3 204 imRecv(receive);
zeutel 7:1f2508ade0a3 205 mbedOperate(receive);
zeutel 5:03d31f0a4943 206 if(millis() - pretime > 5000){//5秒おきにGPS送信
zeutel 5:03d31f0a4943 207 if(sequence != 1){
zeutel 5:03d31f0a4943 208 waitTime.start();
zeutel 5:03d31f0a4943 209 waitTime.reset();
zeutel 5:03d31f0a4943 210 while(gps.result==false){//GPSが利用可能になるまでまつ
zeutel 5:03d31f0a4943 211 gps.getgps();
zeutel 5:03d31f0a4943 212 if(waitTime.read() > 1){
zeutel 5:03d31f0a4943 213 waitTime.stop();
zeutel 5:03d31f0a4943 214 break;
zeutel 5:03d31f0a4943 215 }
Nozaryo 3:1f93c4510fb1 216 }
zeutel 7:1f2508ade0a3 217 sprintf(sendData,"%08.5fE,%08.5fN",gps.longitude,gps.latitude);
zeutel 7:1f2508ade0a3 218 imSend(sendData);
zeutel 5:03d31f0a4943 219 }
zeutel 5:03d31f0a4943 220 pretime=millis();
Nozaryo 3:1f93c4510fb1 221 }
zeutel 0:cebf1f73ffd5 222 }
zeutel 0:cebf1f73ffd5 223 }
zeutel 0:cebf1f73ffd5 224
zeutel 0:cebf1f73ffd5 225 void init(FILE **file){
zeutel 0:cebf1f73ffd5 226 //im920
zeutel 0:cebf1f73ffd5 227 im920.init();
zeutel 0:cebf1f73ffd5 228 wait(1);
zeutel 0:cebf1f73ffd5 229
zeutel 0:cebf1f73ffd5 230 //MMTXS03
zeutel 0:cebf1f73ffd5 231 if(lps331.isAvailable()) {
zeutel 7:1f2508ade0a3 232 imSend("MM-TXS03,OK");
zeutel 0:cebf1f73ffd5 233 } else {
zeutel 7:1f2508ade0a3 234 imSend("MM-TXS03,NG");
zeutel 0:cebf1f73ffd5 235 }
zeutel 0:cebf1f73ffd5 236
zeutel 0:cebf1f73ffd5 237 //MM9DS1
zeutel 7:1f2508ade0a3 238 //lsm.begin();
zeutel 7:1f2508ade0a3 239 uint16_t WhoIsLsm = lsm.begin();
zeutel 7:1f2508ade0a3 240 if(WhoIsLsm==0x683D){
zeutel 7:1f2508ade0a3 241 imSend("MM-9DS1,OK");
zeutel 7:1f2508ade0a3 242 }else{
zeutel 7:1f2508ade0a3 243 imSend("MM-9DS1,NG");
zeutel 7:1f2508ade0a3 244 }
zeutel 7:1f2508ade0a3 245 /*if(lsm.whoAmI()){
zeutel 7:1f2508ade0a3 246 imSend("MM-9DS1,OK");
zeutel 0:cebf1f73ffd5 247 }else {
zeutel 7:1f2508ade0a3 248 imSend("MM-9DS1,NG");
zeutel 7:1f2508ade0a3 249 }*/
zeutel 0:cebf1f73ffd5 250
zeutel 7:1f2508ade0a3 251 //opening shock
zeutel 7:1f2508ade0a3 252 if(accelerometer.getDeviceID()==0xE5) {
zeutel 7:1f2508ade0a3 253 imSend("ADXL345,OK");
zeutel 7:1f2508ade0a3 254 }else{
zeutel 7:1f2508ade0a3 255 imSend("ADXL345,NG");
zeutel 7:1f2508ade0a3 256 }
zeutel 7:1f2508ade0a3 257
Nozaryo 3:1f93c4510fb1 258 wait(1);
zeutel 5:03d31f0a4943 259
zeutel 0:cebf1f73ffd5 260 //SD
zeutel 0:cebf1f73ffd5 261 *file = fopen("/sd/data.csv", "w");
zeutel 0:cebf1f73ffd5 262 if(*file == NULL) {
zeutel 7:1f2508ade0a3 263 imSend("SD,NG");
Nozaryo 3:1f93c4510fb1 264 SDisAvailable = false;
zeutel 0:cebf1f73ffd5 265 }else{
zeutel 7:1f2508ade0a3 266 imSend("SD,OK");
zeutel 5:03d31f0a4943 267 fprintf(*file,"time,temperature,pressure,temperature_hw,pressure_hw,accX,accY,accZ,gyroX,gyroY,gyroZ,magnX,magnY,magnZ,longitude,latitude\r\n");
Nozaryo 3:1f93c4510fb1 268 SDisAvailable = true;
zeutel 0:cebf1f73ffd5 269 }
zeutel 0:cebf1f73ffd5 270
zeutel 0:cebf1f73ffd5 271 //GPS優先度
zeutel 7:1f2508ade0a3 272 /*NVIC_SetPriority(UART3_IRQn,0);
zeutel 0:cebf1f73ffd5 273 gps.getgps();
zeutel 0:cebf1f73ffd5 274 if(gps.result){
zeutel 7:1f2508ade0a3 275 imSend("GPS...OK");
zeutel 5:03d31f0a4943 276 sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
zeutel 7:1f2508ade0a3 277 imSend(sendData);
zeutel 0:cebf1f73ffd5 278 }else{
zeutel 7:1f2508ade0a3 279 imSend("GPS...NG");
zeutel 7:1f2508ade0a3 280 }*/
zeutel 0:cebf1f73ffd5 281
zeutel 0:cebf1f73ffd5 282 //フライトピン
Nozaryo 2:cad76b5be4ba 283 //flightpin.output();
zeutel 0:cebf1f73ffd5 284
zeutel 0:cebf1f73ffd5 285 //各種設定
zeutel 7:1f2508ade0a3 286 lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
zeutel 7:1f2508ade0a3 287 lps331.setDataRate(LPS331_I2C_DATARATE_7HZ);
zeutel 0:cebf1f73ffd5 288 lps331.setActive(true);
zeutel 0:cebf1f73ffd5 289 lsm.setAccelODR(lsm.A_ODR_952);
zeutel 0:cebf1f73ffd5 290 lsm.setGyroODR(lsm.G_ODR_952_BW_100);
zeutel 7:1f2508ade0a3 291 lsm.setMagODR(lsm.M_ODR_80);
zeutel 7:1f2508ade0a3 292
zeutel 7:1f2508ade0a3 293 // These are here to test whether any of the initialization fails. It will print the failure
zeutel 7:1f2508ade0a3 294 /*if (accelerometer.setPowerControl(0x00)){
zeutel 7:1f2508ade0a3 295 pc.printf("didn't intitialize power control\r\n");
zeutel 7:1f2508ade0a3 296 }
zeutel 7:1f2508ade0a3 297 //Full resolution, +/-16g, 4mg/LSB.
zeutel 7:1f2508ade0a3 298 wait(.001);
zeutel 7:1f2508ade0a3 299
zeutel 7:1f2508ade0a3 300 if(accelerometer.setDataFormatControl(0x0B)){
zeutel 7:1f2508ade0a3 301 pc.printf("didn't set data format\r\n");
zeutel 7:1f2508ade0a3 302 }
zeutel 7:1f2508ade0a3 303 wait(.001);*/
zeutel 7:1f2508ade0a3 304
zeutel 7:1f2508ade0a3 305 //3.2kHz data rate.
zeutel 7:1f2508ade0a3 306 if(accelerometer.setDataRate(ADXL345_200HZ)){
zeutel 7:1f2508ade0a3 307 pc.printf("didn't set data rate\r\n");
zeutel 7:1f2508ade0a3 308 }
zeutel 7:1f2508ade0a3 309 wait(.001);
zeutel 7:1f2508ade0a3 310
zeutel 7:1f2508ade0a3 311 //分解能
zeutel 7:1f2508ade0a3 312 accelerometer.setOpticalResolution(0x0B);
zeutel 7:1f2508ade0a3 313
zeutel 7:1f2508ade0a3 314 //測定モード
zeutel 7:1f2508ade0a3 315 if(accelerometer.setPowerControl(MeasurementMode)) {
zeutel 7:1f2508ade0a3 316 pc.printf("didn't set the power control to measurement\r\n");
zeutel 7:1f2508ade0a3 317 }
zeutel 7:1f2508ade0a3 318
zeutel 7:1f2508ade0a3 319
zeutel 0:cebf1f73ffd5 320 }
zeutel 0:cebf1f73ffd5 321
zeutel 0:cebf1f73ffd5 322 void checkingSensors(){
zeutel 0:cebf1f73ffd5 323 //MMTXS03
zeutel 0:cebf1f73ffd5 324 if(lps331.isAvailable()) {
zeutel 7:1f2508ade0a3 325 imSend("MM-TXS03,OK");
zeutel 0:cebf1f73ffd5 326 } else {
zeutel 7:1f2508ade0a3 327 imSend("MM-TXS03,NG");
zeutel 0:cebf1f73ffd5 328 }
zeutel 0:cebf1f73ffd5 329
zeutel 0:cebf1f73ffd5 330 //MM9DS1
zeutel 0:cebf1f73ffd5 331 if(lsm.whoAmI()){
zeutel 7:1f2508ade0a3 332 imSend("MM-9DS1,OK");
zeutel 0:cebf1f73ffd5 333 }else {
zeutel 7:1f2508ade0a3 334 imSend("MM-9DS1,NG");
zeutel 0:cebf1f73ffd5 335 }
zeutel 0:cebf1f73ffd5 336
zeutel 7:1f2508ade0a3 337 //opening shock
zeutel 7:1f2508ade0a3 338 if(accelerometer.getDeviceID()==0xE5) {
zeutel 7:1f2508ade0a3 339 imSend("ADXL345,OK");
zeutel 7:1f2508ade0a3 340 }else{
zeutel 7:1f2508ade0a3 341 imSend("ADXL345,NG");
zeutel 7:1f2508ade0a3 342 }
zeutel 7:1f2508ade0a3 343
zeutel 0:cebf1f73ffd5 344 //GPS
zeutel 7:1f2508ade0a3 345 /*gps.getgps();
zeutel 0:cebf1f73ffd5 346 if(gps.result){
zeutel 7:1f2508ade0a3 347 imSend("GPS...OK");
zeutel 5:03d31f0a4943 348 sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
zeutel 7:1f2508ade0a3 349 imSend(sendData);
zeutel 0:cebf1f73ffd5 350 }else{
zeutel 7:1f2508ade0a3 351 imSend("GPS...NG");
zeutel 7:1f2508ade0a3 352 }*/
zeutel 0:cebf1f73ffd5 353 }
zeutel 0:cebf1f73ffd5 354
zeutel 7:1f2508ade0a3 355 void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings){
zeutel 7:1f2508ade0a3 356 int accels[3];
zeutel 7:1f2508ade0a3 357
zeutel 0:cebf1f73ffd5 358 lsm.readAccel();
zeutel 0:cebf1f73ffd5 359 lsm.readGyro();
zeutel 0:cebf1f73ffd5 360 lsm.readMag();
zeutel 7:1f2508ade0a3 361 *pressure=lps331.getPressure();
zeutel 7:1f2508ade0a3 362 *temperature=lps331.getTemperature();
zeutel 7:1f2508ade0a3 363 /*if(sequence!=1){
zeutel 0:cebf1f73ffd5 364 gps.getgps();
zeutel 7:1f2508ade0a3 365 }*/
zeutel 7:1f2508ade0a3 366 accelerometer.getOutput(accels);
zeutel 7:1f2508ade0a3 367 for(int i=0;i<3;i++){
zeutel 7:1f2508ade0a3 368 openingShockReadings[i]=(int16_t)accels[i];//*0.0392266;
zeutel 0:cebf1f73ffd5 369 }
zeutel 0:cebf1f73ffd5 370 }
zeutel 0:cebf1f73ffd5 371
zeutel 0:cebf1f73ffd5 372 float calcAltitude(float pres, float temp){
zeutel 7:1f2508ade0a3 373 return (pow(1015.5/pres,1/5.257)-1)*(temp+273.15)/0.0065;
zeutel 7:1f2508ade0a3 374 }
zeutel 7:1f2508ade0a3 375
zeutel 7:1f2508ade0a3 376 void imRecv(char *receive){
zeutel 7:1f2508ade0a3 377 im920.poll();
zeutel 7:1f2508ade0a3 378 memset(receive,'\0',sizeof(receive));
zeutel 7:1f2508ade0a3 379 im920.recv(receive,8);
zeutel 7:1f2508ade0a3 380 pc.printf(receive);
zeutel 7:1f2508ade0a3 381 //camera用コマンド認識
zeutel 7:1f2508ade0a3 382 char *camStr=strtok(receive,":");//cam:startというようにコマンド
zeutel 7:1f2508ade0a3 383 if(strncmp(camStr,"cam",sizeof(camStr))==0){
zeutel 7:1f2508ade0a3 384 imSend(receive);
zeutel 7:1f2508ade0a3 385 }
zeutel 7:1f2508ade0a3 386 }
zeutel 7:1f2508ade0a3 387
zeutel 7:1f2508ade0a3 388 void imSend(char *send){
zeutel 7:1f2508ade0a3 389 im920.send(send,strlen(send)+1);
zeutel 7:1f2508ade0a3 390 pc.printf(send);
zeutel 7:1f2508ade0a3 391 pc.printf("\r\n");
zeutel 7:1f2508ade0a3 392 }
zeutel 7:1f2508ade0a3 393
zeutel 7:1f2508ade0a3 394 void mbedOperate(char *command){
zeutel 7:1f2508ade0a3 395 if(strncmp(command,"reset",sizeof(command))==0){
zeutel 7:1f2508ade0a3 396 imSend("Mbed reset");
zeutel 7:1f2508ade0a3 397 NVIC_SystemReset();
zeutel 7:1f2508ade0a3 398 }
zeutel 7:1f2508ade0a3 399
zeutel 7:1f2508ade0a3 400 if(strncmp(command,"retry",sizeof(command))==0){
zeutel 7:1f2508ade0a3 401 imSend("Retry initializing");
zeutel 7:1f2508ade0a3 402 init(&fp);
zeutel 7:1f2508ade0a3 403 wait(1);
zeutel 7:1f2508ade0a3 404 }
zeutel 7:1f2508ade0a3 405
zeutel 7:1f2508ade0a3 406 if(strncmp(command,"check",sizeof(command))==0){
zeutel 7:1f2508ade0a3 407 imSend("Check");
zeutel 7:1f2508ade0a3 408 checkingSensors();
zeutel 7:1f2508ade0a3 409 }
zeutel 7:1f2508ade0a3 410 }
zeutel 7:1f2508ade0a3 411
zeutel 7:1f2508ade0a3 412 void sendDatas(float time, float temp, float pres, float ax, float ay, float az, float gx, float gy, float gz){
zeutel 7:1f2508ade0a3 413 sprintf(sendData,"data1,%.3f,%.3f,%.3f",time,temp,pres);
zeutel 7:1f2508ade0a3 414 imSend(sendData);
zeutel 7:1f2508ade0a3 415 sprintf(sendData,"data2,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",ax,ay,az,gx,gy,gz);
zeutel 7:1f2508ade0a3 416 imSend(sendData);
zeutel 0:cebf1f73ffd5 417 }