![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS精度実験
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library2 XBee2 agzIDLIST_2016 mbed
Diff: main.cpp.orig
- Revision:
- 3:693ea9476763
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