aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver4_1

change kalman

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_ver4 by aigamozu

main.cpp@1:b2b950b916ce, 2015-05-13 (annotated)

Committer:
kityann
Date:
Wed May 13 04:09:16 2015 +0000
Revision:
1:b2b950b916ce
Parent:
0:daab5accfd83
Child:
2:886fac7f4399
2015/05/13

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:daab5accfd83 1 /**********************************************/
kityann 0:daab5accfd83 2 //
kityann 0:daab5accfd83 3 //
kityann 0:daab5accfd83 4 //
kityann 0:daab5accfd83 5 // Program name: Aigamozu ROBOT
kityann 0:daab5accfd83 6 // Author: Mineta Kizuku
kityann 0:daab5accfd83 7 //
kityann 0:daab5accfd83 8 //
kityann 0:daab5accfd83 9 /**********************************************/
kityann 0:daab5accfd83 10
kityann 0:daab5accfd83 11 /**********************************************/
kityann 0:daab5accfd83 12 //更新情報
kityann 0:daab5accfd83 13 //2015/05/11
kityann 0:daab5accfd83 14 //ロボットプログラムの作成
kityann 0:daab5accfd83 15 //
kityann 0:daab5accfd83 16 //
kityann 0:daab5accfd83 17 //
kityann 0:daab5accfd83 18 /**********************************************/
kityann 0:daab5accfd83 19
kityann 0:daab5accfd83 20 #include "mbed.h"
kityann 0:daab5accfd83 21 #include "XBee.h"
kityann 0:daab5accfd83 22 #include "MBed_Adafruit_GPS.h"
kityann 0:daab5accfd83 23 #include "AigamozuControlPackets.h"
kityann 0:daab5accfd83 24 #include "agzIDLIST.h"
kityann 0:daab5accfd83 25 #include "aigamozuSetting.h"
kityann 0:daab5accfd83 26 #include "Kalman.h"
kityann 0:daab5accfd83 27
kityann 0:daab5accfd83 28 //************ID Number*****************
kityann 0:daab5accfd83 29 //Robot ID: 'A' ~ 'Z'
kityann 0:daab5accfd83 30 //Base ID: 'a' ~ 'a'
kityann 0:daab5accfd83 31 //manager ID: '0'(数字のゼロ)
kityann 0:daab5accfd83 32 //
kityann 1:b2b950b916ce 33 const char MyID = 'd';
kityann 0:daab5accfd83 34 //************ID Number*****************
kityann 0:daab5accfd83 35
kityann 0:daab5accfd83 36 /////////////////////////////////////////
kityann 0:daab5accfd83 37 //
kityann 0:daab5accfd83 38 //Pin Setting
kityann 0:daab5accfd83 39 //
kityann 0:daab5accfd83 40 /////////////////////////////////////////
kityann 0:daab5accfd83 41 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:daab5accfd83 42
kityann 0:daab5accfd83 43
kityann 0:daab5accfd83 44 /////////////////////////////////////////
kityann 0:daab5accfd83 45 //
kityann 0:daab5accfd83 46 //Connection Setting
kityann 0:daab5accfd83 47 //
kityann 0:daab5accfd83 48 /////////////////////////////////////////
kityann 0:daab5accfd83 49
kityann 0:daab5accfd83 50 //Serial Connect Setting: PC <--> mbed
kityann 0:daab5accfd83 51 Serial pc(USBTX, USBRX);
kityann 0:daab5accfd83 52
kityann 0:daab5accfd83 53 //Serial Connect Setting: GPS <--> mbed
kityann 0:daab5accfd83 54 Serial * gps_Serial;
kityann 0:daab5accfd83 55
kityann 0:daab5accfd83 56 //Serial Connect Setting: XBEE <--> mbed
kityann 0:daab5accfd83 57 XBee xbee(p13,p14);
kityann 0:daab5accfd83 58 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:daab5accfd83 59
kityann 0:daab5accfd83 60 //set up GPS module
kityann 0:daab5accfd83 61
kityann 0:daab5accfd83 62 //set up AigamozuControlPackets library
kityann 0:daab5accfd83 63 AigamozuControlPackets agz(agz_motorShield);
kityann 0:daab5accfd83 64
kityann 0:daab5accfd83 65
kityann 0:daab5accfd83 66 /////////////////////////////////////////
kityann 0:daab5accfd83 67 //
kityann 0:daab5accfd83 68 //For Kalman data
kityann 0:daab5accfd83 69 //
kityann 0:daab5accfd83 70 /////////////////////////////////////////
kityann 0:daab5accfd83 71 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:daab5accfd83 72 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:daab5accfd83 73 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:daab5accfd83 74 double y[2],x[2][2]={0};
kityann 0:daab5accfd83 75 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
kityann 0:daab5accfd83 76
kityann 0:daab5accfd83 77 /////////////////////////////////////////
kityann 0:daab5accfd83 78 //
kityann 0:daab5accfd83 79 //Address List
kityann 0:daab5accfd83 80 //
kityann 0:daab5accfd83 81 /////////////////////////////////////////
kityann 0:daab5accfd83 82 XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
kityann 0:daab5accfd83 83 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
kityann 0:daab5accfd83 84 XBeeAddress64(BASE5_32H,BASE5_32L)};
kityann 0:daab5accfd83 85 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
kityann 0:daab5accfd83 86 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:daab5accfd83 87
kityann 0:daab5accfd83 88
kityann 0:daab5accfd83 89 /////////////////////////////////////////
kityann 0:daab5accfd83 90 //
kityann 0:daab5accfd83 91 //Send_Status
kityann 0:daab5accfd83 92 //
kityann 0:daab5accfd83 93 //リクエストがきたとき、自分の位置情報などを返信する
kityann 0:daab5accfd83 94 /////////////////////////////////////////
kityann 0:daab5accfd83 95 void Send_Status(char SenderIDc){
kityann 0:daab5accfd83 96 XBeeAddress64 send_Address;
kityann 0:daab5accfd83 97 if(SenderIDc == '0'){
kityann 0:daab5accfd83 98 send_Address = manager_Address;
kityann 0:daab5accfd83 99 }
kityann 0:daab5accfd83 100 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 0:daab5accfd83 101 send_Address = robot_Address[SenderIDc - 'A'];
kityann 0:daab5accfd83 102 }
kityann 0:daab5accfd83 103 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 0:daab5accfd83 104 send_Address = base_Address[SenderIDc - 'a'];
kityann 0:daab5accfd83 105 }
kityann 0:daab5accfd83 106 //send normal data
kityann 0:daab5accfd83 107 //Create GPS Infomation Packet
kityann 0:daab5accfd83 108 agz.createReceiveStatusCommand(MyID,SenderIDc,
kityann 0:daab5accfd83 109 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:daab5accfd83 110 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:daab5accfd83 111 agz.get_agzCov_lati(),agz.get_agzCov_longi());
kityann 0:daab5accfd83 112
kityann 0:daab5accfd83 113 //debug***************************************************
kityann 0:daab5accfd83 114 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
kityann 0:daab5accfd83 115 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
kityann 0:daab5accfd83 116 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
kityann 0:daab5accfd83 117 agz.get_agzCov_lati(),agz.get_agzCov_longi()
kityann 0:daab5accfd83 118 );
kityann 0:daab5accfd83 119 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
kityann 0:daab5accfd83 120 printf("\n");
kityann 0:daab5accfd83 121 //debug end***************************************************
kityann 0:daab5accfd83 122
kityann 0:daab5accfd83 123 //Select Destination
kityann 0:daab5accfd83 124 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
kityann 0:daab5accfd83 125 //Send -> Base
kityann 0:daab5accfd83 126 xbee.send(tx64request);
kityann 0:daab5accfd83 127 }
kityann 0:daab5accfd83 128
kityann 0:daab5accfd83 129 /////////////////////////////////////////
kityann 0:daab5accfd83 130 //
kityann 0:daab5accfd83 131 //Get GPS function
kityann 0:daab5accfd83 132 //
kityann 0:daab5accfd83 133 /////////////////////////////////////////
kityann 0:daab5accfd83 134
kityann 0:daab5accfd83 135 void Get_GPS(Adafruit_GPS *myGPS){
kityann 0:daab5accfd83 136 static bool flag = true;
kityann 0:daab5accfd83 137
kityann 0:daab5accfd83 138 if (myGPS->fix) {
kityann 0:daab5accfd83 139 agz.nowStatus = GPS_AVAIL;
kityann 0:daab5accfd83 140
kityann 0:daab5accfd83 141 if(flag){//初期値設定
kityann 0:daab5accfd83 142 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
kityann 0:daab5accfd83 143 flag = false;
kityann 0:daab5accfd83 144 x[0][0]=(double)myGPS->latitudeL;
kityann 0:daab5accfd83 145 x[0][1]=(double)myGPS->longitudeL;
kityann 0:daab5accfd83 146 }
kityann 0:daab5accfd83 147 }
kityann 0:daab5accfd83 148
kityann 0:daab5accfd83 149 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
kityann 0:daab5accfd83 150 return;
kityann 0:daab5accfd83 151 }
kityann 0:daab5accfd83 152 //Kalman Filter
kityann 0:daab5accfd83 153 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
kityann 0:daab5accfd83 154
kityann 0:daab5accfd83 155 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
kityann 0:daab5accfd83 156 agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
kityann 0:daab5accfd83 157 }
kityann 0:daab5accfd83 158 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:daab5accfd83 159
kityann 0:daab5accfd83 160 }
kityann 1:b2b950b916ce 161
kityann 1:b2b950b916ce 162 /////////////////////////////////////////
kityann 1:b2b950b916ce 163 //
kityann 1:b2b950b916ce 164 //Get Status
kityann 1:b2b950b916ce 165 //
kityann 1:b2b950b916ce 166 /////////////////////////////////////////
kityann 1:b2b950b916ce 167 void Get_Status(char SenderIDc,uint8_t *packetdata){
kityann 1:b2b950b916ce 168
kityann 1:b2b950b916ce 169 //マネージャからデータが来たとき
kityann 1:b2b950b916ce 170 if(SenderIDc == '0'){
kityann 1:b2b950b916ce 171 printf("get manager Status\n");
kityann 1:b2b950b916ce 172 }
kityann 1:b2b950b916ce 173 //他のロボットからデータが来たとき
kityann 1:b2b950b916ce 174 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
kityann 1:b2b950b916ce 175 printf("get other robots Status\n");
kityann 1:b2b950b916ce 176 }
kityann 1:b2b950b916ce 177 //基地局からデータが来たとき
kityann 1:b2b950b916ce 178 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
kityann 1:b2b950b916ce 179 printf("Get Base data\n");
kityann 1:b2b950b916ce 180 int id = SenderIDc - 'a';
kityann 1:b2b950b916ce 181 agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
kityann 1:b2b950b916ce 182 agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
kityann 1:b2b950b916ce 183
kityann 1:b2b950b916ce 184 //debug
kityann 1:b2b950b916ce 185 for(int i = 0;i < 4;i++){
kityann 1:b2b950b916ce 186 printf("BASE:%d\n",i);
kityann 1:b2b950b916ce 187 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
kityann 1:b2b950b916ce 188 agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
kityann 1:b2b950b916ce 189 agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
kityann 1:b2b950b916ce 190 );
kityann 1:b2b950b916ce 191 }
kityann 1:b2b950b916ce 192 }
kityann 1:b2b950b916ce 193 }
kityann 1:b2b950b916ce 194
kityann 1:b2b950b916ce 195 /////////////////////////////////////////
kityann 1:b2b950b916ce 196 //
kityann 1:b2b950b916ce 197 //Send_Request_to_base
kityann 1:b2b950b916ce 198 //
kityann 1:b2b950b916ce 199 /////////////////////////////////////////
kityann 1:b2b950b916ce 200 void Send_Request_Base(int basenumber){
kityann 1:b2b950b916ce 201 printf("send\n");
kityann 1:b2b950b916ce 202 agz.createRequestCommand(MyID, basenumber);
kityann 1:b2b950b916ce 203 //Select Destination
kityann 1:b2b950b916ce 204 ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
kityann 1:b2b950b916ce 205 //Send -> Base
kityann 1:b2b950b916ce 206 xbee.send(tx64request);
kityann 1:b2b950b916ce 207 }
kityann 0:daab5accfd83 208
kityann 0:daab5accfd83 209
kityann 0:daab5accfd83 210 /////////////////////////////////////////
kityann 0:daab5accfd83 211 //
kityann 0:daab5accfd83 212 //Kalman Processing
kityann 0:daab5accfd83 213 //
kityann 0:daab5accfd83 214 /////////////////////////////////////////
kityann 0:daab5accfd83 215
kityann 0:daab5accfd83 216 void get_K(){
kityann 0:daab5accfd83 217 double temp[2][2]={
kityann 0:daab5accfd83 218 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:daab5accfd83 219 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:daab5accfd83 220 };
kityann 0:daab5accfd83 221 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:daab5accfd83 222 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:daab5accfd83 223 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:daab5accfd83 224 }
kityann 0:daab5accfd83 225
kityann 0:daab5accfd83 226
kityann 0:daab5accfd83 227 void get_x(){
kityann 0:daab5accfd83 228 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:daab5accfd83 229 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:daab5accfd83 230 }
kityann 0:daab5accfd83 231
kityann 0:daab5accfd83 232
kityann 0:daab5accfd83 233 void get_sigma(){
kityann 0:daab5accfd83 234 double temp[2][2];
kityann 0:daab5accfd83 235 for(int i=0;i<2;i++) {
kityann 0:daab5accfd83 236 for(int j=0;j<2;j++) {
kityann 0:daab5accfd83 237 for(int k=0;k<2;k++) {
kityann 0:daab5accfd83 238 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:daab5accfd83 239 }
kityann 0:daab5accfd83 240 }
kityann 0:daab5accfd83 241 }
kityann 0:daab5accfd83 242 for(int i = 0;i < 2;i++){
kityann 0:daab5accfd83 243 for(int j = 0;j < 2;j++){
kityann 0:daab5accfd83 244 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:daab5accfd83 245 }
kityann 0:daab5accfd83 246 }
kityann 0:daab5accfd83 247 }
kityann 0:daab5accfd83 248
kityann 0:daab5accfd83 249 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
kityann 0:daab5accfd83 250 y[0] = Latitude;
kityann 0:daab5accfd83 251 y[1] = Longitude;
kityann 0:daab5accfd83 252 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:daab5accfd83 253 get_K();
kityann 0:daab5accfd83 254 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:daab5accfd83 255 get_x();
kityann 0:daab5accfd83 256 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:daab5accfd83 257 get_sigma();
kityann 0:daab5accfd83 258
kityann 0:daab5accfd83 259
kityann 0:daab5accfd83 260 //kousinn
kityann 0:daab5accfd83 261 for(int i = 0;i < 2;i++){
kityann 0:daab5accfd83 262 for(int j = 0;j < 2;j++){
kityann 0:daab5accfd83 263 K[0][i][j]=K[1][i][j];
kityann 0:daab5accfd83 264 x[0][i]=x[1][i];
kityann 0:daab5accfd83 265 sigma[0][i][j]=sigma[1][i][j];
kityann 0:daab5accfd83 266 }
kityann 0:daab5accfd83 267 }
kityann 0:daab5accfd83 268
kityann 0:daab5accfd83 269 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
kityann 0:daab5accfd83 270 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
kityann 0:daab5accfd83 271 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
kityann 0:daab5accfd83 272 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
kityann 0:daab5accfd83 273
kityann 0:daab5accfd83 274 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
kityann 0:daab5accfd83 275 }
kityann 0:daab5accfd83 276
kityann 0:daab5accfd83 277
kityann 0:daab5accfd83 278 /////////////////////////////////////////
kityann 0:daab5accfd83 279 //
kityann 0:daab5accfd83 280 //Main Processing
kityann 0:daab5accfd83 281 //
kityann 0:daab5accfd83 282 /////////////////////////////////////////
kityann 0:daab5accfd83 283 int main() {
kityann 0:daab5accfd83 284 //start up time
kityann 0:daab5accfd83 285 wait(3);
kityann 0:daab5accfd83 286 //set pc frequency to 57600bps
kityann 0:daab5accfd83 287 pc.baud(PC_BAUD_RATE);
kityann 0:daab5accfd83 288 //set xbee frequency to 57600bps
kityann 0:daab5accfd83 289 xbee.begin(XBEE_BAUD_RATE);
kityann 0:daab5accfd83 290
kityann 0:daab5accfd83 291
kityann 0:daab5accfd83 292 //GPS setting
kityann 0:daab5accfd83 293 gps_Serial = new Serial(p28,p27);
kityann 0:daab5accfd83 294 Adafruit_GPS myGPS(gps_Serial);
kityann 0:daab5accfd83 295 Timer refresh_Timer;
kityann 0:daab5accfd83 296 const int refresh_Time = 2000; //refresh time in ms
kityann 1:b2b950b916ce 297 Timer collect_Timer;
kityann 1:b2b950b916ce 298 const int collect_Time = 2000; //refresh time in ms
kityann 0:daab5accfd83 299 myGPS.begin(GPS_BAUD_RATE);
kityann 0:daab5accfd83 300
kityann 0:daab5accfd83 301 char SenderIDc;
kityann 1:b2b950b916ce 302 int basenumber=0;
kityann 0:daab5accfd83 303 //GPS Send Command
kityann 0:daab5accfd83 304 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:daab5accfd83 305 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:daab5accfd83 306 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:daab5accfd83 307
kityann 0:daab5accfd83 308 wait(2);
kityann 0:daab5accfd83 309
kityann 0:daab5accfd83 310 //interrupt start
kityann 0:daab5accfd83 311 refresh_Timer.start();
kityann 1:b2b950b916ce 312 collect_Timer.start();
kityann 1:b2b950b916ce 313
kityann 0:daab5accfd83 314 printf("start\n");
kityann 0:daab5accfd83 315
kityann 0:daab5accfd83 316 while (true) {
kityann 0:daab5accfd83 317
kityann 0:daab5accfd83 318 //Check Xbee Buffer Available
kityann 0:daab5accfd83 319 xbee.readPacket();
kityann 0:daab5accfd83 320
kityann 0:daab5accfd83 321 if (xbee.getResponse().isAvailable()) {
kityann 0:daab5accfd83 322 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 323 uint8_t *buf = zbRx.getFrameData();
kityann 0:daab5accfd83 324
kityann 0:daab5accfd83 325 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:daab5accfd83 326 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:daab5accfd83 327 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
kityann 0:daab5accfd83 328 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
kityann 0:daab5accfd83 329 SenderIDc = buf1[5];//送信元のIDを取得する
kityann 0:daab5accfd83 330 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
kityann 0:daab5accfd83 331
kityann 0:daab5accfd83 332 //Check Command Type
kityann 0:daab5accfd83 333 switch(Command_type){
kityann 0:daab5accfd83 334 //Get Request command
kityann 0:daab5accfd83 335 case STATUS_REQUEST:{
kityann 0:daab5accfd83 336 Send_Status(SenderIDc);
kityann 0:daab5accfd83 337 break;
kityann 0:daab5accfd83 338 }
kityann 1:b2b950b916ce 339 case RECEIVE_STATUS:{
kityann 1:b2b950b916ce 340 Get_Status(SenderIDc,buf1);
kityann 1:b2b950b916ce 341 break;
kityann 1:b2b950b916ce 342 }
kityann 0:daab5accfd83 343 default:{
kityann 0:daab5accfd83 344 break;
kityann 0:daab5accfd83 345 }
kityann 0:daab5accfd83 346 }//endswitch
kityann 0:daab5accfd83 347 }//endifZB_RX_RESPONSE
kityann 0:daab5accfd83 348 }//endifisAvailable
kityann 0:daab5accfd83 349
kityann 0:daab5accfd83 350 myGPS.read();
kityann 0:daab5accfd83 351 //recive gps module
kityann 0:daab5accfd83 352 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:daab5accfd83 353 if ( myGPS.newNMEAreceived() ) {
kityann 0:daab5accfd83 354 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:daab5accfd83 355 continue;
kityann 0:daab5accfd83 356 }
kityann 0:daab5accfd83 357 }
kityann 1:b2b950b916ce 358 //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
kityann 0:daab5accfd83 359 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:daab5accfd83 360 refresh_Timer.reset();
kityann 0:daab5accfd83 361 Get_GPS(&myGPS);
kityann 0:daab5accfd83 362
kityann 0:daab5accfd83 363 }
kityann 1:b2b950b916ce 364
kityann 1:b2b950b916ce 365 //一定時間ごとに基地局のGPSデータを取得し、AigamozuControlPacketsないのagzBasePointとagzBasePointKalmanに格納する
kityann 1:b2b950b916ce 366 if (collect_Timer.read_ms() >= collect_Time) {
kityann 1:b2b950b916ce 367 collect_Timer.reset();
kityann 1:b2b950b916ce 368 Send_Request_Base(basenumber);
kityann 1:b2b950b916ce 369 basenumber++;
kityann 1:b2b950b916ce 370 if(basenumber > 4)basenumber=0;
kityann 1:b2b950b916ce 371 }
kityann 0:daab5accfd83 372 }
kityann 0:daab5accfd83 373 }