![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2015/05/13
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Base_ver1_2 by
Diff: main.cpp
- Revision:
- 1:ee2713435312
- Parent:
- 0:eee5e3d906ce
- Child:
- 2:d97700414618
diff -r eee5e3d906ce -r ee2713435312 main.cpp --- a/main.cpp Tue May 12 01:03:31 2015 +0000 +++ b/main.cpp Tue May 12 11:36:34 2015 +0000 @@ -102,9 +102,21 @@ } //send normal data //Create GPS Infomation Packet - agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), + agz.createReceiveStatusCommand(MyID,SenderIDc, + agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), agz.get_agzCov_lati(),agz.get_agzCov_longi()); + + //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 @@ -124,7 +136,7 @@ agz.nowStatus = GPS_AVAIL; if(flag){//初期値設定 - if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){ + if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){ flag = false; x[0][0]=(double)myGPS->latitudeL; x[0][1]=(double)myGPS->longitudeL; @@ -137,8 +149,8 @@ //Kalman Filter Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS); - agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL); - agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL); + agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL); + agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL); } else agz.nowStatus = GPS_UNAVAIL; @@ -267,7 +279,7 @@ switch(Command_type){ //Get Request command case STATUS_REQUEST:{ - Send_Status(SenderIDc); + Send_Status(SenderIDc); break; } default:{ @@ -290,13 +302,7 @@ if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); Get_GPS(&myGPS); - - 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() - ); - + } } } \ No newline at end of file