![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS精度実験
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library2 XBee2 agzIDLIST_2016 mbed
main.cpp.orig
- Committer:
- s1200058
- Date:
- 2016-10-14
- Revision:
- 3:693ea9476763
File content as of revision 3:693ea9476763:
/**********************************************/ // // // // 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(); } } }