Diff: main.cpp

Revision:

11:2fc33706edd4

Parent:

10:c60fec9b4e1b

Child:

12:ed1e3282e053

Child:

13:7947a88c4e39

diff -r c60fec9b4e1b -r 2fc33706edd4 main.cpp
--- a/main.cpp	Sun Aug 30 07:01:19 2020 +0000
+++ b/main.cpp	Mon Aug 31 11:21:27 2020 +0000
@@ -3,9 +3,7 @@
 #include "SDFileSystem.h"
 #include "GPS.h"
 
- 
-
-GPS gps(PA_9, PA_10);       // tx, rx
+GPS gps(PA_9, PA_10);       //TX(GPS_RX),RX(GPS_TX)
 DigitalIn FlightPin(PB_0);
 
 Serial pc(USBTX, USBRX, 9600);//パソコン側からもマイコン⇒IM920のデータはみれる
@@ -57,38 +55,27 @@
         bmx.getAcc();//加速度の取得
         bmx.getGyro();//角力加速度の取得
         bmx.getMag();//地磁気の取得
-        
-        gps.getgps();
-        int x = (float)(gps.longtitude-135.0)*1000000;
-        int y = (float)(gps.latitude-34.0)*1000000;
-        if(x==NULL)x=0;
-        if(y==NULL)y=0;
-    
-        
-        //float hokui_n=0;
-        //float tokei_n=0;
-    
-        //getGPS(hokui_n,tokei_n);//GPS取得
-        
-       // printf("%f,%f\n\r",*hokui_n,*tokei_n);
-        
-        fprintf(fp,"%f,%f,%f,%f,%f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f\n\r",gps.longtitude,gps.latitude,(bmx.accel[0]/512)*9.8,
-        (bmx.accel[1]/512)*9.8,(bmx.accel[2]/512)*9.8,(bmx.gyroscope[0]*125)/2048,
-        (bmx.gyroscope[1]*125)/2048,(bmx.gyroscope[2]*125)/2048,bmx.magnet[0],bmx.magnet[1],bmx.magnet[2]);
         int a[3],b[3],c[3];
         for(int i=0;i<3;i++){
             a[i]=bmx.accel[i];
-            if(a[i]<0)a[i] += 4096;//負値の場合+4096
             b[i]=bmx.gyroscope[i];
-            if(b[i]<0)b[i] += 4096;//負値の場合+4096
             c[i]=bmx.magnet[i];
-            if(c[i]<0)c[i] += 4096;//負値の場合+4096
-                     
-            }
+            }        
+        
         //GPS部分
+        //gps.getgps();
+        int x = (gps.longtitude-135.0f)*1000000;
+        int y = (gps.latitude-34.0f)*1000000;
+        if(x==NULL)x=0;
+        if(y==NULL)y=0;
+        int north=x * 1000000;
+        int east=y * 1000000;
         
-        //fprintf(fp,"北緯:%f,東経:%f,加速度:%f,%f,%f,ジャイロ:%2.4f,%2.4f,%2.4f\n\r",hokui_n,tokei_n,(bmx.accel[0]/512)*9.8,(bmx.accel[1]/512)*9.8,(bmx.accel[2]/512)*9.8,(bmx.gyroscope[0]*125)/2048,(bmx.gyroscope[1]*125)/2048,(bmx.gyroscope[2]*125)/2048);
-
+        //SDカードへの書き込み
+        fprintf(fp,"%f,%f,%f,%f,%f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f\n\r",gps.longtitude,gps.latitude,(a[0]/512)*9.8,
+        (a[1]/512)*9.8f,(a[2]/512)*9.8f,(b[0]*125)/2048.0f,
+        (b[1]*125)/2048.0f,(b[2]*125)/2048.0f,c[0]/3.0f,c[1]/3.0f,c[2]/3.0f);
+        
         //IM920部分
         //ノード番号(00~FF)を設定.データを送るごとに1増やす．
         uart.printf("STNN %02x\r\n",j);
@@ -97,12 +84,8 @@
         
         uart.printf("TXDA ");//可変長データ送信コマンド
         
-        uart.printf("%04x%04x%04x",a[0],a[1],a[2]);//加速度の書き出し
-        uart.printf("%04x%04x%04x",b[0],b[1],b[2]);//角加速度の書き出し
-        
-        
-        int north=x * 1000000;
-        int east=y * 1000000;
+        uart.printf("%04hX%04hX%04hX",a[0],a[1],a[2]);//加速度の書き出し
+        uart.printf("%04hX%04hX%04hX",b[0],b[1],b[2]);//角加速度の書き出し
         
         uart.printf("%08x%08x",east,north);//GPS書き出し
         
@@ -111,64 +94,7 @@
 
         wait(0.34);//IM920長距離モードの場合，3パケット/sが理論上最大速度．
         }
-        
-            
-        //fclose(fp);
-        
-        return 0;
-        
     }
-        
-        
-/*void getGPS(float *hokui_n,float *tokei_n) {         //関数getGPS定義
-            unsigned char c = gps.getc();
-            if( c=='$' || i == 256){
-                mode = 0;
-                i = 0;
-            }
-            if(mode==0){
-                if((gps_data[i]=c) != '\r'){
-                i++;
-            }else{
-                gps_data[i]='\0';
-                // pc.printf("%s\r\n",gps_data);
-                if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
-                    if(rlock==1){
-                          //  pc.printf("Status:Lock(%d)\n\r",rlock);
-                            //logitude
-                            d_tokei= int(tokei/100);
-                            m_tokei= (tokei-d_tokei*100)/60;
-                            g_tokei= d_tokei+m_tokei;
-                            //pc.printf("Log:%4.5f,",g_tokei);                            //Latitude
-                            d_hokui=int(hokui/100);
-                            m_hokui=(hokui-d_hokui*100)/60;
-                            g_hokui=d_hokui+m_hokui;
-                            // pc.printf("Lat:%4.5f\n\r",g_hokui);
-                                                  
-                            *hokui_n=g_hokui;
-                            *tokei_n=g_tokei;
-            }
-            /*if(rlock==0){
-          //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
-          //pc.printf("%s",gps_data);
-                *hokui_n=0;
-                *tokei_n=0;
-        }
-      }//if
-        *hokui_n=hokui;
-        *tokei_n=tokei;
-    }   //else
-  }     //L.123 if
- }*/
-
-/*void callback () {
-    int i;
-    char buf[65];
-
-    //i = im920.recv(buf, 64);
-    buf[i] = 0;
-    printf("recv: '%s' (%d)\r\n", buf, i);
-}*/
 
 void FlightPinDriver(){
     
@@ -181,7 +107,7 @@
         }
 
 void nichrome_ON(){
-    printf("テグスカット!\n\r");
+    printf("cut!\n\r");
     fet1=1;
     wait(2.0);//テグスを切るまでにかかる時間
     fet1=0;
@@ -190,6 +116,6 @@
     }
 
 void buzzer(){
-    printf("ブザー作動\n\r");
+    printf("buzzer\n\r");
     fet2=1;
     }
\ No newline at end of file