GPS精度実験

Dependencies:   ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library2 XBee2 agzIDLIST_2016 mbed

Files at this revision

API Documentation at this revision

Comitter:
s1200058
Date:
Fri Oct 14 07:58:59 2016 +0000
Parent:
2:575479bf1302
Commit message:
test;

Changed in this revision

MBed_Adafruit-GPS-Library2.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp.orig Show annotated file Show diff for this revision Revisions of this file
diff -r 575479bf1302 -r 693ea9476763 MBed_Adafruit-GPS-Library2.lib
--- a/MBed_Adafruit-GPS-Library2.lib	Wed Sep 14 11:03:42 2016 +0000
+++ b/MBed_Adafruit-GPS-Library2.lib	Fri Oct 14 07:58:59 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library2/#7b7e9dc49edd
+https://developer.mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library2/#f1c5757af8dd
diff -r 575479bf1302 -r 693ea9476763 main.cpp
--- a/main.cpp	Wed Sep 14 11:03:42 2016 +0000
+++ b/main.cpp	Fri Oct 14 07:58:59 2016 +0000
@@ -361,13 +361,13 @@
     const int collect_Time = 1000; 
 
     char SenderIDc;
+     wait_ms(2000);
     //GPS Send Command
-    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_ALLDATA);
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
-
-    wait_ms(2000);
-
+        wait_ms(2000);
+        
     //interrupt start
     refresh_Timer.start();
     auto_Timer.start();
@@ -377,6 +377,11 @@
     printf("start\n");
 
     agz.auto_count = 0;
+    
+ //   myGPS.count_[0] = 3;
+ //   myGPS.count_[1] = 3;
+ //   myGPS.count_[2] = 3;
+ myGPS.print_ok = 0;
 
     while (true) {
 
@@ -403,7 +408,7 @@
                     }
                     case STATUS_REQUEST: {
                         Send_Status(SenderIDc);
-                        printf("%c\n", SenderIDc);
+                      //  printf("%c\n", SenderIDc);
                         break;
                     }
                     case CHANGE_MODE: {
@@ -422,7 +427,9 @@
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
 
-        myGPS.read();
+
+             myGPS.read();
+
         //recive gps module
         //check if we recieved a new message from GPS, if so, attempt to parse it,
         if ( myGPS.newNMEAreceived() ) {
@@ -451,11 +458,11 @@
         
 
         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
-        if (refresh_Timer.read_ms() >= 1000){
-            refresh_Timer.reset();
+//   if (refresh_Timer.read_ms() >= 1000){
+        //    refresh_Timer.reset();
             //print_gps(count);
 
-        }
+     //   }
 
         
     }
diff -r 575479bf1302 -r 693ea9476763 main.cpp.orig
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Fri Oct 14 07:58:59 2016 +0000
@@ -0,0 +1,467 @@
+/**********************************************/
+//
+//
+//
+//  Program name: Aigamozu ROBOT
+//  Author: Mineta Kizuku
+//
+//
+//
+/**********************************************/
+
+/**********************************************/
+//更新情報
+//2015/05/11
+//ロボットプログラムの作成
+//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
+//
+//2015/05/13 Yokokawa
+//何回目のGPSでとられたGPSか確認するようにしました。
+//
+//2015/05/15
+//プログラムcreateReceiveStatusCommand()にて
+//Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
+//
+//2015/05//17
+//Send_Status関数内を変更:基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
+//基地局以外には現在のモードを送信するようにしてある
+//要確認!!!!
+//
+//2015/05/17
+//Get_GPS()の中身longitudeの範囲138〜140に変更
+//
+//2015/05/19
+//autoモードのとき、三十秒前進・三秒右に転回に変更した。
+//
+//2015/05/24
+//Kalmanフィルターを十進数で計算するようにした。
+//Kalmanフィルターの計算式を変更した。
+//set_kalmanを追加した。
+//
+/**********************************************/
+
+#include "mbed.h"
+#include "XBee.h"
+#include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
+#include "agzIDLIST.h"
+#include "aigamozuSetting.h"
+#include "Kalman.h"
+#include "math.h"
+
+#define SIGMA_MIN 0.0001
+#define KALMAN_START_TIME 5
+
+//************ID Number*****************
+//Robot   ID: 'A' ~ 'Z'
+//Base    ID: 'a' ~ 'z'
+//manager ID: '0'(数字のゼロ)
+//
+const char MyID = 'B';
+const char Collect_Number = 'a';
+//************ID Number*****************
+
+/////////////////////////////////////////
+//
+//Pin Setting
+//
+/////////////////////////////////////////
+VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+
+
+/////////////////////////////////////////
+//
+//Connection Setting
+//
+/////////////////////////////////////////
+
+//Serial Connect Setting: PC <--> mbed
+Serial pc(USBTX, USBRX);
+
+//Serial Connect Setting: GPS <--> mbed
+Serial * gps_Serial;
+
+//Serial Connect Setting: XBEE <--> mbed
+XBee xbee(p13,p14);
+ZBRxResponse zbRx = ZBRxResponse();
+
+//set up GPS module
+
+//set up AigamozuControlPackets library
+AigamozuControlPackets agz(agz_motorShield);
+
+
+/////////////////////////////////////////
+//
+//For Kalman data
+//
+/////////////////////////////////////////
+#define FIRST_S1 0.0001
+#define FIRST_S2 0.000001
+#define COUNTER_MAX 10000
+double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
+double s2x_cur=FIRST_S1,s2x_prev=FIRST_S1,s2y_cur=FIRST_S1,s2y_prev=FIRST_S1;//緯度経度のの時刻tと時刻t-1での共分散
+double s2_R=FIRST_S2;//GPSセンサの分散
+double Kx=0,Ky=0;//カルマンゲイン
+double zx,zy;//観測値
+void Kalman(double Latitude,double Longitude);
+int change = 0;
+
+/*
+LocalFileSystem local("local");  // マウントポイントを定義(ディレクトリパスになる)
+FILE *fp;
+char filename[16] = "/local/out0.txt";
+*/
+/////////////////////////////////////////
+//
+//Plus Speed
+//
+//MNUAL_MODEの時にスピードを変える
+/////////////////////////////////////////
+void Plus_Speed(uint8_t *packetdata)
+{
+
+    if(agz.nowMode == MANUAL_MODE) {
+        agz.changeSpeed(packetdata);
+    }
+
+}
+
+
+/////////////////////////////////////////
+//
+//Send Status
+//
+//リクエストがきたとき、自分の位置情報などを返信する
+/////////////////////////////////////////
+void Send_Status(char SenderIDc)
+{
+    XBeeAddress64 send_Address;
+    if(SenderIDc == '0') {
+        send_Address = manager_Address;
+        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
+    }
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
+        send_Address = robot_Address[SenderIDc - 'A'];
+        //Create GPS Infomation Packet
+        agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
+    }
+    if(SenderIDc >= 'a' && SenderIDc <= 'z') {
+        send_Address = base_Address[SenderIDc - 'a'];
+
+        agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
+    }
+    //send normal data
+
+    /*
+       //debug***************************************************
+       printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
+               agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+               agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+               agz.get_agzCov_lati(),agz.get_agzCov_longi()
+               );
+       for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
+           printf("\n");
+       //debug end***************************************************
+    */
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+
+}
+
+/////////////////////////////////////////
+//
+//Get GPS function
+//
+/////////////////////////////////////////
+
+void Get_GPS(Adafruit_GPS *myGPS)
+{
+    static int flag = 0;
+//    static int save_counter = 0;
+
+    if (myGPS->fix) {
+
+        agz.nowStatus = GPS_AVAIL;
+
+            /*            if(save_counter < 10){
+                            fp = fopen(filename, "a");
+                            fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
+                                change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                agz.get_agzCov_lati(),agz.get_agzCov_longi());
+                            fclose(fp);
+
+                            if((flag - 16) % 500 == 0){
+                                filename[10]++;
+                                save_counter++;
+                            }
+                        }
+            */
+
+    } else agz.nowStatus = GPS_UNAVAIL;
+
+}
+
+/////////////////////////////////////////
+//
+//New Mode
+//
+/////////////////////////////////////////
+
+void New_Mode(uint8_t *packetdata)
+{
+
+    //bool result;
+    agz.changeMode(packetdata);
+
+}
+
+/////////////////////////////////////////
+//
+//Get Status
+//
+/////////////////////////////////////////
+void Get_Status(char SenderIDc,uint8_t *packetdata)
+{
+    int id = 0;
+
+        //printf("Get data\n");
+        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        agz.set_agzCov_from_packet(&packetdata[45],&packetdata[53]);
+
+
+        printf("%d, %f, %f\n",
+               SenderIDc, agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
+              );
+    
+
+}
+
+
+/////////////////////////////////////////
+//
+//Send_Request
+//
+/////////////////////////////////////////
+void Send_Request(int number)
+{
+
+    agz.createRequestCommand(MyID, number);
+    //Select Destination!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    //基地局の場合
+    if('a' <= number && number <= 'z'){
+    number = number - 'a';
+    //printf("Send b:%d\n",number);
+    ZBTxRequest tx64request(base_Address[number],agz.packetData,agz.getPacketLength());
+    //Send
+    xbee.send(tx64request);
+    }
+    //ロボットの場合
+    if('A' <= number && number <= 'Z'){
+    number = number - 'A';
+    //printf("Send r:%d\n",MyID);
+    ZBTxRequest tx64request(robot_Address[number],agz.packetData,agz.getPacketLength());
+    //Send
+    xbee.send(tx64request);
+    }
+
+}
+
+
+void print_gps(int count)
+{
+
+    printf("%d times:\n", count);
+    printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
+
+}
+
+
+/////////////////////////////////////////
+//
+//Kalman Processing
+//
+/////////////////////////////////////////
+void calc_Kalman()
+{
+    //calc Kalman gain
+    Kx = s2x_prev/(s2x_prev+s2_R);
+    Ky = s2y_prev/(s2y_prev+s2_R);
+    //estimate
+    x_cur = x_prev + Kx*(zx-x_prev);
+    y_cur = y_prev + Ky*(zy-y_prev);
+    //calc sigma
+    s2x_cur = s2x_prev-Kx*s2x_prev;
+    s2y_cur = s2y_prev-Ky*s2y_prev;
+
+}
+
+void Kalman(double Latitude,double Longitude)
+{
+
+    zx = Latitude;
+    zy = Longitude;
+
+    calc_Kalman();
+
+    //更新
+    x_prev = x_cur;
+    y_prev = y_cur;
+    s2x_prev = s2x_cur;
+    s2y_prev = s2y_cur;
+
+    //agzPontKalmanとagzCovに格納する
+    agz.set_agzPointKalman_lati(x_cur);
+    agz.set_agzPointKalman_longi(y_cur);
+    agz.set_agzCov(s2x_cur,s2y_cur);
+
+}
+
+/////////////////////////////////////////
+//
+//Main Processing
+//
+/////////////////////////////////////////
+int main()
+{
+    //start up time
+    wait(3);
+    //set pc frequency to 57600bps
+    pc.baud(PC_BAUD_RATE);
+    //set xbee frequency to 57600bps
+    xbee.begin(XBEE_BAUD_RATE);
+
+
+    //GPS setting
+    gps_Serial = new Serial(p28,p27);
+    Adafruit_GPS myGPS(gps_Serial);
+    Timer refresh_Timer;
+    Timer auto_Timer;
+//    const int straight = 8000, turning = 12000, wait = 10000;
+
+    myGPS.begin(GPS_BAUD_RATE);
+
+    Timer collect_Timer;
+    const int collect_Time = 1000; 
+
+    char SenderIDc;
+    //GPS Send Command
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+    wait_ms(2000);
+
+    //interrupt start
+    refresh_Timer.start();
+    auto_Timer.start();
+    agz.Move_Timer.start();
+    collect_Timer.start();
+    agz.Automove_Timer.start();
+    printf("start\n");
+
+    agz.auto_count = 0;
+
+    while (true) {
+
+        //Check Xbee Buffer Available
+        xbee.readPacket();
+
+        if (xbee.getResponse().isAvailable()) {
+            xbee.getResponse().getZBRxResponse(zbRx);
+            uint8_t *buf = zbRx.getFrameData();
+
+            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+                xbee.getResponse().getZBRxResponse(zbRx);
+                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
+                uint8_t *buf1 = &buf[11];//データの部分のみを格納する
+                SenderIDc = buf1[5];//送信元のIDを取得する
+                char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
+
+                //Check Command Type
+                switch(Command_type) {
+                        //Get Request command
+                    case MANUAL: {
+                        Plus_Speed(buf);
+                        break;
+                    }
+                    case STATUS_REQUEST: {
+                        Send_Status(SenderIDc);
+                        //printf("%c\n", SenderIDc);
+                        break;
+                    }
+                    case CHANGE_MODE: {
+                        New_Mode(buf);
+                        break;
+                    }
+                    case RECEIVE_STATUS: {
+                        //printf("GET");
+                        Get_Status(SenderIDc,buf1);
+                        break;
+                    }
+                    default: {
+                        break;
+                    }
+                }//endswitch
+            }//endifZB_RX_RESPONSE
+        }//endifisAvailable
+
+        myGPS.read();
+        //recive gps module
+        //check if we recieved a new message from GPS, if so, attempt to parse it,
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;   
+            }else{
+ 
+            }    
+            if(strstr(myGPS.lastNMEA(), "$GPGSA")){
+                //myGPS.GPGSAdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPGSAdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPGGA")){
+                //myGPS.GPGGAdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPGGAdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPRMC")){
+                //myGPS.GPRMCdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPRMCdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPGSV")){
+                //myGPS.GPRMCdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPRMCdata);
+            } 
+        }
+        
+
+        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
+      if(refresh_Timer.read_ms() >= 3000 && myGPS.print_ok == 1){
+            printf("%s\n",myGPS.GPGGAdata);               
+            printf("%s\n",myGPS.GPGSAdata);
+            printf("%s\n",myGPS.GPRMCdata);
+            printf("%s\n",myGPS.GPGSVdataA);
+            printf("%s\n",myGPS.GPGSVdataB);
+            printf("%s\n",myGPS.GPGSVdataC);
+            printf("%s\n",myGPS.GPGSVdataD);
+            refresh_Timer.reset();
+    }
+
+        
+    }
+
+}
\ No newline at end of file