Takeyuki Nogami / Mbed 2 deprecated Azimuth_judge

2019/03 Hello oshima by TAKEYUKI

Dependencies:   mbed

main.cpp@0:a892d87fe2c9, 2019-03-12 (annotated)

Committer:
3311smtwtfs
Date:
Tue Mar 12 14:25:30 2019 +0000
Revision:
0:a892d87fe2c9
wowwow!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
3311smtwtfs 0:a892d87fe2c9 1 #include "mbed.h"
3311smtwtfs 0:a892d87fe2c9 2 #include "math.h"
3311smtwtfs 0:a892d87fe2c9 3 #define PAI 3.141592653589
3311smtwtfs 0:a892d87fe2c9 4 /*mbedの設定*/
3311smtwtfs 0:a892d87fe2c9 5 Serial pc(USBTX, USBRX);
3311smtwtfs 0:a892d87fe2c9 6 Serial gps(p9 , p10);
3311smtwtfs 0:a892d87fe2c9 7 Serial musen(p28 , p27);
3311smtwtfs 0:a892d87fe2c9 8 Ticker tic_gps;
3311smtwtfs 0:a892d87fe2c9 9 /*グローバル変数*/
3311smtwtfs 0:a892d87fe2c9 10 int cnt_gps = 0;
3311smtwtfs 0:a892d87fe2c9 11 float now_lat,now_lon,goal_lat,goal_lon,MAX_ALT;
3311smtwtfs 0:a892d87fe2c9 12 /*プロトタイプ宣言*/
3311smtwtfs 0:a892d87fe2c9 13 void _SendGPS();
3311smtwtfs 0:a892d87fe2c9 14 char c[256];
3311smtwtfs 0:a892d87fe2c9 15 float _DMS2DEG(float raw_data);
3311smtwtfs 0:a892d87fe2c9 16 /*main関数*/
3311smtwtfs 0:a892d87fe2c9 17 int main(){
3311smtwtfs 0:a892d87fe2c9 18 pc.baud(115200);
3311smtwtfs 0:a892d87fe2c9 19 pc.printf("Hello OSHIMA\r\n");
3311smtwtfs 0:a892d87fe2c9 20 tic_gps.attach(&_SendGPS, 1.0);
3311smtwtfs 0:a892d87fe2c9 21 int i = 0;
3311smtwtfs 0:a892d87fe2c9 22 while(1){
3311smtwtfs 0:a892d87fe2c9 23 c[i] = musen.getc();
3311smtwtfs 0:a892d87fe2c9 24 if(c[i] == '\n'){
3311smtwtfs 0:a892d87fe2c9 25 break;
3311smtwtfs 0:a892d87fe2c9 26 }
3311smtwtfs 0:a892d87fe2c9 27 }
3311smtwtfs 0:a892d87fe2c9 28 sscanf(c,"Lat:%f,Lon:%f,MAX_ALT:%f",&goal_lat,&goal_lon,&MAX_ALT);
3311smtwtfs 0:a892d87fe2c9 29 //度をradに変換
3311smtwtfs 0:a892d87fe2c9 30 float delta_lon = (PAI * (goal_lon - now_lon)/180);
3311smtwtfs 0:a892d87fe2c9 31 now_lat = (PAI * (now_lat)/180);
3311smtwtfs 0:a892d87fe2c9 32 now_lon = (PAI * (now_lon)/180);
3311smtwtfs 0:a892d87fe2c9 33 goal_lat = (PAI * (goal_lat)/180);
3311smtwtfs 0:a892d87fe2c9 34 goal_lon = (PAI * (goal_lon)/180);
3311smtwtfs 0:a892d87fe2c9 35 //距離方位角計算
3311smtwtfs 0:a892d87fe2c9 36 float dist = 6378.137 * acos( sin(goal_lat) * sin(now_lat) + cos(goal_lat) * cos(now_lat) * cos(delta_lon));
3311smtwtfs 0:a892d87fe2c9 37 float azi = (180.0/3.1415926535)*atan2(sin(delta_lon), cos(now_lat) * tan(goal_lat) - sin(now_lat)*cos(delta_lon));
3311smtwtfs 0:a892d87fe2c9 38 if (azi < 0){
3311smtwtfs 0:a892d87fe2c9 39 azi = azi + 360.0;
3311smtwtfs 0:a892d87fe2c9 40 }
3311smtwtfs 0:a892d87fe2c9 41 pc.printf("距離：%f,方位：%f,/r/n",dist,azi);
3311smtwtfs 0:a892d87fe2c9 42 }
3311smtwtfs 0:a892d87fe2c9 43 void _SendGPS(){
3311smtwtfs 0:a892d87fe2c9 44 char gps_data[256];
3311smtwtfs 0:a892d87fe2c9 45 while(1){
3311smtwtfs 0:a892d87fe2c9 46 if(gps.readable()){
3311smtwtfs 0:a892d87fe2c9 47 gps_data[cnt_gps] = gps.getc();
3311smtwtfs 0:a892d87fe2c9 48 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
3311smtwtfs 0:a892d87fe2c9 49 cnt_gps = 0;
3311smtwtfs 0:a892d87fe2c9 50 memset(gps_data,'\0',256);
3311smtwtfs 0:a892d87fe2c9 51 }else if(gps_data[cnt_gps] == '\r'){
3311smtwtfs 0:a892d87fe2c9 52 float world_time, lon_east, lat_north;
3311smtwtfs 0:a892d87fe2c9 53 int rlock, sat_num;
3311smtwtfs 0:a892d87fe2c9 54 char lat,lon;
3311smtwtfs 0:a892d87fe2c9 55 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
3311smtwtfs 0:a892d87fe2c9 56 if(rlock==1){
3311smtwtfs 0:a892d87fe2c9 57 now_lat = _DMS2DEG(lat_north);
3311smtwtfs 0:a892d87fe2c9 58 now_lon = _DMS2DEG(lon_east);
3311smtwtfs 0:a892d87fe2c9 59 //pc.printf("%s\r\n",gps_data);
3311smtwtfs 0:a892d87fe2c9 60 //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",now_lat,now_lon,world_time,sat_num);
3311smtwtfs 0:a892d87fe2c9 61 int japan_time = int(world_time) - 9;
3311smtwtfs 0:a892d87fe2c9 62 pc.printf("Lat:%f,Lon:%f\r\n",now_lat,now_lon);
3311smtwtfs 0:a892d87fe2c9 63 break;
3311smtwtfs 0:a892d87fe2c9 64 }
3311smtwtfs 0:a892d87fe2c9 65 else{
3311smtwtfs 0:a892d87fe2c9 66 //pc.printf("%s\r\n",gps_data);
3311smtwtfs 0:a892d87fe2c9 67 pc.printf("NoGPSSignal\r\n");
3311smtwtfs 0:a892d87fe2c9 68 break;
3311smtwtfs 0:a892d87fe2c9 69 }
3311smtwtfs 0:a892d87fe2c9 70 }else{
3311smtwtfs 0:a892d87fe2c9 71 //ffpc.printf("No_Satellite_signal\r\n");
3311smtwtfs 0:a892d87fe2c9 72 }
3311smtwtfs 0:a892d87fe2c9 73 }else{
3311smtwtfs 0:a892d87fe2c9 74 cnt_gps++;
3311smtwtfs 0:a892d87fe2c9 75 }
3311smtwtfs 0:a892d87fe2c9 76 }
3311smtwtfs 0:a892d87fe2c9 77 }
3311smtwtfs 0:a892d87fe2c9 78 }
3311smtwtfs 0:a892d87fe2c9 79 float _DMS2DEG(float raw_data){
3311smtwtfs 0:a892d87fe2c9 80 int d=(int)(raw_data/100);
3311smtwtfs 0:a892d87fe2c9 81 float m=(raw_data-(float)d*100);
3311smtwtfs 0:a892d87fe2c9 82 return (float)d+m/60;
3311smtwtfs 0:a892d87fe2c9 83 }
3311smtwtfs 0:a892d87fe2c9 84