File content as of revision 0:029ef267b1bc:

/*
 * 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);
            }
           }
    
     }
}