#include "mbed.h"
#include "TextLCD0420.h"
#include "math.h"
#define ON 1
#define OFF 0
#define PI 3.14159
double gy,gx;
double ry,rx,rv,rd;
double dy,dx,dr,dd;
int np=0;
int wp=0;
double wy[16]={0};
double wx[16]={0};
// GPS受信用変数,ポインタ
char c; // GPSほか読み取りのための文字列バッファ
char *header;
char *latitude, *longitude, *knot, *direct;
char gps_data[256];
char ns,ew;
double latit,longit,kn; //緯度,経度,ノット保存用の変数
double latitB,longitB,rdB; //差分で方位計算する時に前の座標保存,方位保管用
double dy2,dx2,rd2; //一つ前の北緯,東経の差分(m),差分で求めた方位
DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
DigitalIn sw1(p5);
TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,16,2); // rs, rw, e, d0, d1, d2, d3
Serial gps(p9,p10);
LocalFileSystem local("local");
Ticker flipper;
FILE *fp;
float g_hokui,g_tokei;
int fp_count=0;
void gps_rec() {
mled0=ON;
fprintf(fp,"%4.6f,%3.6f,\n",dx,dy);
wait(1.0);
mled0=OFF;
fp_count++;
}
int main() {
int i,rlock=0,stn=0; float time,hokui,tokei; float d_hokui,m_hokui,d_tokei,m_tokei; int h_time=0,m_time=0,s_time=0; int rec_flag=0; gps.baud(9600); lcd.cls(); lcd.printf("*** GPS GT-720F ****\n"); lcd.locate(0,1); lcd.printf("File open...\n"); wait(1.0); fp=fopen("/local/GPS.txt","a"); if(!fp || sw1==0){ lcd.cls(); lcd.printf("Can't Loging ... \n"); lcd.printf("System close...\n"); lcd.printf(">\n"); exit(1); } lcd.printf("System start...\n"); lcd.printf("Loging ready ok...\n"); fprintf(fp,"\n"); wait(1.0); while (1) { if(sw1==0){ fclose(fp); lcd.cls(); lcd.printf("System close...\n"); lcd.printf(">\n"); exit(1); } i=0; while(gps.getc()!='$'){ } while( (gps_data[i]=gps.getc()) != '\r'){ i++; if(i==256){ lcd.printf("*** Data read Error! ***\n"); i=255; break; } } gps_data[i]='\0'; //test /* Test data rlock=1; stn=3; hokui=3532.25024; //=>35.537502 tokei=13751.86820;//=>137.864471 time=114107.046; */ if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){ if(rlock >= 1){ //hokui d_hokui=int(hokui/100); m_hokui=(hokui-d_hokui*100)/60; g_hokui=d_hokui+m_hokui; //tokei d_tokei=int(tokei/100); m_tokei=(tokei-d_tokei*100)/60; g_tokei=d_tokei+m_tokei; //g_hokui=int(hokui/100)+(hokui-int(hokui/100))/60; //g_tokei=int(tokei/100)+(tokei-int(tokei/100))/60; //time set h_time=int(time/10000); m_time=int((time-h_time*10000)/100); s_time=int(time-h_time*10000-m_time*100); h_time=h_time+9;//UTC =>JST // Record start if(rec_flag==0){ flipper.attach(&gps_rec, 10.0); rec_flag=1; fprintf(fp,"JST %2d:%2d:%2d\n",h_time,m_time,s_time); lcd.cls(); mled0=ON; lcd.printf("Loging start...."); wait(5.0); mled0=ON; } lcd.cls(); lcd.locate(0,0); lcd.printf("*GPS JST %2d:%2d:%2d",h_time,m_time,s_time); lcd.locate(0,1); lcd.printf("Lk(%d),St(%d),%d",rlock,stn,fp_count); //Latitude=Hokui lcd.locate(0,2); lcd.printf("Lat/d:%4.6f",dy); // Logitude=tokei lcd.locate(0,3); lcd.printf("Log/d:%4.6f",dx); } else{ flipper.detach(); rec_flag=0; lcd.cls(); lcd.locate(0,0); lcd.printf("*** GPS GT-720F ***"); lcd.locate(0,1); lcd.printf("Lk(%d),St(%d)",rlock,stn); lcd.locate(0,2); for(i=0;i<40;i++){ lcd.printf("%c",gps_data[i]); } } }//if }//while // GPS信号の数値返還
//void gps_val(){
latitB=latit;
longitB=longit; // 一つ前の値を保管
latit=atof(latitude+2); // 文字列を実数に変換.有効数字が足りないので度単位を省略
longit=atof(longitude+3); // 文字列を実数に変換.有効数字が足りないので度単位を省略
kn=atof(knot);
rv=kn*0.51; // ノット単位をメートル単位に変換
rdB=rd; // 一つ前の値を保管
rd=atof(direct); // 方位を数値変換
}
// 現在地とウェイポイントとの相対関係を計算
void weypoint(){
// 緯度経度の数値変換,目標地点との差分を計算
dy=(wy[np] - latit ) *1860;
dx=(wx[np] - longit)*1560;
// 目標地点までのと距離を計算 dr=sqrt(pow(dy,2)+pow(dx,2)); // 目標地点への方向を計算 dd = atan(dx / dy); // 北に対する角度を求める(±π/2範囲) dd=dd*57; // ラジアン->度に変換 dd*(180/pai) // 0-360度に変換 if (dx > 0 && dy < 0) dd = 180 + dd; else if(dx < 0 && dy < 0) dd = 180 + dd; else if(dx < 0 && dy > 0) dd = 360 + dd; else; // GPSが正しい方位を出力しない場合は、前の座標との差分で計算 dy2=(latit -latitB) *1860; dx2=(longit-longitB)*1560; if (dy2==0 || dx2==0) { // 緯度または経度のどちらかの変化が0の時 if (dy2==0){ if (dx2<0) rd2= 270; else rd2=90; } // rd2= 0, 90, 180,270 のいずれかを直接与える. if (dx2==0){ if (dy2<0) rd2=-180; else rd2=0; } // dy2,dx2のどちらも0の時はrd2=0 } else { // 目標地点への方向を計算 rd2 = atan(dx2 / dy2); // 北に対する角度を求める(±π/2範囲) rd2=rd2*57; // ラジアン->度に変換 dd*(180/pai) // 0-360度に変換 if (dx2 > 0 && dy2 < 0) rd2 = 180 + rd2; else if(dx2 < 0 && dy2 < 0) rd2 = 180 + rd2; else if(dx2 < 0 && dy2 > 0) rd2 = 360 + rd2; else; // 方位偏差の計算し,現在の進行方向から±180度の範囲に直す dd=dd-rd; if (dd>180) dd=dd-360; else if (dd<-180) dd=dd+360; }
}//main
GPSを受信し現在地からあらかじめ設定した目標地点までの差をだしサーボを傾けたいのですが、うまく動作しません。いまは差をだすことができればと思っていますが、知識不足のためご指摘いただけると助かります。よろしくお願いします。使用モジュールはマイコン「mbed」,GPS「GT-720F」,ディスプレイ「キャラクタLCD0420」
あなたの回答
tips
プレビュー