Diff: main.cpp

Revision:

0:029ef267b1bc

Child:

1:2992de38cf3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,272 @@
+/*
+ * MAG3110 Sensor Library for mbed
+ * TODO: Add proper header
+ */
+
+
+#include "mbed.h"
+#include "MotionSensor.h"
+#include "MAG3110.h"
+#include "math.h"
+//#include "SDFileSystem.h"
+
+//SDFileSystem sd(p5, p6, p7, p8, "sd");
+//Serial pc(USBTX,USBRX);
+Serial pc(p9,p10);
+
+int16_t x,y,z;
+LocalFileSystem local("local");
+MAG3110 mag(p28,p27);
+Serial gps(p13,p14);
+
+DigitalIn local_on(p20);
+DigitalIn local_del(p19);
+DigitalIn north(p21);
+DigitalIn east(p22);
+DigitalIn south(p23);
+DigitalIn west(p24);
+DigitalIn cds(p29);
+
+
+DigitalOut bit1(LED4);
+DigitalOut bit2(LED3);
+DigitalOut bit3(LED2);
+DigitalOut bit4(LED1);
+
+char gps_c[250];
+int offset_x,offset_y;
+int x_max,y_max,x_min,y_min;
+int count_offset,count_gps;
+int a,b,c,d,e,f,g;
+float x_target,y_target,d_target;
+float longitude_target = 36.11707;
+float latitude_target = 139.4720;
+float utc_time,latitude,longitude,speed,course,utc_date,gps_magnetic;
+char pos_status;
+float direction,x_dat,y_dat;
+char angle_c;
+
+
+void init(){
+    
+    x_max = -32500;//x,yの最大値を初期化
+    y_max = -32500;
+    x_min = 32500;
+    y_min = 32500;
+    
+}
+
+int gps_save(){
+               a=0;
+               count_gps = 0;
+               pc.printf("sequence2\n");
+               
+               while(a <= 159 && count_gps < 3)
+                  {  
+                   bit4=1;
+                   
+                   for(a=0; gps_c[a-1] != '\r' && gps_c[a] != '\n'; a++)
+                       {
+                         gps_c[a] = gps.getc();
+                         pc.printf("%c \n",gps_c[a]);
+                       }
+                       
+                   bit4=0;
+                   
+                   bit2=1;
+                   if(memcmp(gps_c,"$GPRMC",6) == 0)
+                       {  
+                           sscanf(gps_c,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W",&utc_time,&pos_status,&latitude,&longitude,&speed,&course,&utc_date,&gps_magnetic);
+                           pc.printf("$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude,longitude,speed,course,utc_date,gps_magnetic);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                           //mkdir("/sd/gps", 0777);
+                           
+                            if(local_on == 1)
+                                  {
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude,longitude,speed,course,utc_date,gps_magnetic);
+                                          fclose(fp); 
+                                  }
+                            bit3=0;      
+                            count_gps++;
+                        }
+                        
+                      /*else if(memcmp(gps_c,"$GPGGA",6) == 0)
+                         { sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude,&longitude);
+                           pc.printf("$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude,longitude);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                          // mkdir("/sd/gps", 0777);
+                           if(local_on == 1){
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude,longitude);
+                                          fclose(fp); }
+                           bit3=0;
+                           count_gps++;
+                        }*/
+                        
+                     else if(memcmp(gps_c,"$GPGLL",6) == 0)
+                         { sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude,&longitude);
+                           pc.printf("$GPGLL,%f,N,%f,E \n",latitude,longitude);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                           //mkdir("/sd/gps", 0777);
+                           
+                            if(local_on == 1)
+                                {
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude,longitude);
+                                          fclose(fp); 
+                                }
+                           bit3=0;
+                           count_gps++;
+                          }
+                    bit2=0;
+               }
+               
+               
+              return 0;
+}
+
+int offset(int x_rd,int y_rd){
+    
+    if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0){
+        init();}//最大値、最小値に0が入った時初期化
+    
+    if(x_max<x_rd && x_rd != 0){
+        x_max=x_rd;}
+        
+    if(y_max<y_rd && y_rd != 0){
+        y_max=y_rd;}
+    
+    if(x_min>x_rd && x_rd != 0){
+        x_min=x_rd;}
+        
+    if(y_min>y_rd && y_rd != 0){
+        y_min=y_rd;}
+        
+    offset_x = ((abs(x_min)+abs(x_max))/2);
+    offset_y = ((abs(y_min)+abs(y_max))/2);
+    
+    if(count_offset < 301){
+           count_offset++;}
+    if(count_offset > 300){//300個分のサンプ取得
+           x_dat = x + offset_x;
+           y_dat = y - offset_y;}
+        
+    return 0;    
+}
+
+void cal_gps(){
+           y_target = (latitude_target-latitude)*111319.49;
+           x_target = (longitude_target-longitude)*cos(latitude*(3.1415926535/180))*111319.49;
+           d_target = sqrt(pow(x,2.0)*pow(y,2.0));     
+}
+
+void cal_con(){
+           
+           direction = (atan(y_dat/x_dat))*57.2958;
+           //direction = (atan(y_cal/x_cal))*57.2958;
+           
+           /*if(0 < direction && 89 > direction){
+               angle_c = 'N';}
+               
+           else if(90 < direction && 179 > direction){
+               angle_c = 'E';}
+               
+           else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
+               angle_c = 'S';}
+               
+           else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
+               angle_c = 'W';}
+        
+           else{ angle_c = '?';}*/        
+           
+           if(north == 1){
+               angle_c = 'N';}
+               
+           else if(east == 1){
+               angle_c = 'E';}
+               
+           else if(south == 1){
+               angle_c = 'S';}
+               
+           else if(west == 1){
+               angle_c = 'W';}
+        
+           else{ angle_c = '?';}    
+    
+}
+
+void con_save(){
+
+    bit1=1;//led4を1にしてデータを保存
+    FILE *fp = fopen("/local/commpas_new.txt","a");
+               fprintf(fp,"%c,%f,%f,%f,%f\n",angle_c,d_target,x_dat,y_dat,direction);
+               //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
+               fclose(fp);
+    bit1=0;    
+}
+
+void dele(){
+    
+    bit2=1;//led3を1
+               
+    remove("/local/commpas_new.txt");
+    remove("/local/gps.txt");
+    
+    bit2=0;
+}
+
+int main(){
+    
+    mag.enable();//コンパス有効化
+    mag.sampleRate(0x40);
+    init(); //初期化
+    
+    while(1){
+           
+           if(cds == 0){
+           while(1){
+           //void disable(void);
+           //uint32_t whoAmI(void);
+           //uint32_t dataReady(void);
+           mag.getX(&x);
+           mag.getY(&y);
+           mag.getZ(&z);
+           //void getX(float * x);
+           //void getY(float * y);
+           //void getZ(float * z);
+           //void getAxis(MotionSensorDataCounts &data);
+           //void getAxis(MotionSensorDataUnits &data);
+           //void readRegs(int addr, uint8_t * data, int len);
+           offset(x,y);//x,y オフセット計算
+           //cal(x_dat,y_dat);
+           cal_con();
+           cal_gps();
+           if(local_on == 1){con_save();}//pin20がhighの時、データ保存
+           if(local_del == 1){dele();}//pin19がhighの時、保存したデータを消去
+           gps_save();
+           pc.printf("<%c> x: %d , y: %d , x_dat: %f , y_dat: %f , direction: %f \n",angle_c,x,y,x_dat,y_dat,direction);    
+           //pc.printf("x_dat: %d  y_dat: %d  x: %d  offset: %d x_max: %d  x_min: %d\n",x_dat,y_dat,x,offset,x_max,x_min);
+            }
+           }
+    
+     }
+}
\ No newline at end of file