cansat program1
Dependencies: ADXL345 BME280 HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST cansat mbed
Fork of Cansat_program4_1 by
main.cpp@4:0fc7221e2f79, 2015-07-23 (annotated)
- Committer:
- aoki0731
- Date:
- Thu Jul 23 09:18:20 2015 +0000
- Revision:
- 4:0fc7221e2f79
- Parent:
- 3:0bd9ad37f319
- Child:
- 5:ba883a4bddc3
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kityann | 0:1fcc61be1dcf | 1 | /**********************************************/ |
kityann | 0:1fcc61be1dcf | 2 | //更新情報 |
kityann | 0:1fcc61be1dcf | 3 | |
kityann | 0:1fcc61be1dcf | 4 | /**********************************************/ |
kityann | 0:1fcc61be1dcf | 5 | |
kityann | 0:1fcc61be1dcf | 6 | #include "mbed.h" |
kityann | 0:1fcc61be1dcf | 7 | #include "XBee.h" |
kityann | 0:1fcc61be1dcf | 8 | #include "MBed_Adafruit_GPS.h" |
kityann | 1:f1f7413ae6bd | 9 | //#include "AigamozuControlPackets.h" |
kityann | 0:1fcc61be1dcf | 10 | #include "agzIDLIST.h" |
kityann | 0:1fcc61be1dcf | 11 | #include "aigamozuSetting.h" |
kityann | 1:f1f7413ae6bd | 12 | #include "HMC5883L.h" |
kityann | 1:f1f7413ae6bd | 13 | #include "VNH5019.h" |
kityann | 3:0bd9ad37f319 | 14 | #include "cansat.h" |
kityann | 0:1fcc61be1dcf | 15 | #include "math.h" |
kityann | 0:1fcc61be1dcf | 16 | |
kityann | 0:1fcc61be1dcf | 17 | #define SIGMA_MIN 0.0001 |
kityann | 0:1fcc61be1dcf | 18 | |
kityann | 0:1fcc61be1dcf | 19 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 20 | // |
kityann | 0:1fcc61be1dcf | 21 | //Pin Setting |
kityann | 0:1fcc61be1dcf | 22 | // |
kityann | 0:1fcc61be1dcf | 23 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 24 | VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); |
kityann | 0:1fcc61be1dcf | 25 | |
kityann | 0:1fcc61be1dcf | 26 | |
kityann | 0:1fcc61be1dcf | 27 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 28 | // |
kityann | 0:1fcc61be1dcf | 29 | //Connection Setting |
kityann | 0:1fcc61be1dcf | 30 | // |
kityann | 0:1fcc61be1dcf | 31 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 32 | |
kityann | 0:1fcc61be1dcf | 33 | //Serial Connect Setting: PC <--> mbed |
kityann | 0:1fcc61be1dcf | 34 | Serial pc(USBTX, USBRX); |
kityann | 0:1fcc61be1dcf | 35 | |
kityann | 0:1fcc61be1dcf | 36 | //Serial Connect Setting: GPS <--> mbed |
kityann | 0:1fcc61be1dcf | 37 | Serial * gps_Serial; |
kityann | 0:1fcc61be1dcf | 38 | |
kityann | 0:1fcc61be1dcf | 39 | //Serial Connect Setting: XBEE <--> mbed |
kityann | 0:1fcc61be1dcf | 40 | XBee xbee(p13,p14); |
kityann | 0:1fcc61be1dcf | 41 | ZBRxResponse zbRx = ZBRxResponse(); |
kityann | 0:1fcc61be1dcf | 42 | |
kityann | 0:1fcc61be1dcf | 43 | //set up GPS module |
kityann | 0:1fcc61be1dcf | 44 | |
kityann | 0:1fcc61be1dcf | 45 | //set up AigamozuControlPackets library |
kityann | 1:f1f7413ae6bd | 46 | //AigamozuControlPackets agz(agz_motorShield); |
kityann | 3:0bd9ad37f319 | 47 | CanSat cansat(agz_motorShield); |
kityann | 0:1fcc61be1dcf | 48 | |
kityann | 0:1fcc61be1dcf | 49 | |
kityann | 0:1fcc61be1dcf | 50 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 51 | // |
kityann | 0:1fcc61be1dcf | 52 | //For Kalman data |
kityann | 0:1fcc61be1dcf | 53 | // |
kityann | 0:1fcc61be1dcf | 54 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 55 | #define FIRST_S2_1 1.0e-8 |
kityann | 0:1fcc61be1dcf | 56 | #define FIRST_S2_2 1.0e-6 |
kityann | 0:1fcc61be1dcf | 57 | #define COUNTER_MAX 10000 |
kityann | 0:1fcc61be1dcf | 58 | #define ERROR_RANGE 0.001 |
kityann | 0:1fcc61be1dcf | 59 | |
kityann | 0:1fcc61be1dcf | 60 | double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値 |
kityann | 0:1fcc61be1dcf | 61 | double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散 |
kityann | 0:1fcc61be1dcf | 62 | double s2_R=FIRST_S2_2;//GPSセンサの分散 |
kityann | 0:1fcc61be1dcf | 63 | double s2_Q=FIRST_S2_2; |
kityann | 0:1fcc61be1dcf | 64 | double Kx=0,Ky=0;//カルマンゲイン |
kityann | 0:1fcc61be1dcf | 65 | double zx,zy;//観測値 |
kityann | 0:1fcc61be1dcf | 66 | void Kalman(double Latitude,double Longitude); |
kityann | 0:1fcc61be1dcf | 67 | int change = 0; |
kityann | 0:1fcc61be1dcf | 68 | |
kityann | 0:1fcc61be1dcf | 69 | |
kityann | 0:1fcc61be1dcf | 70 | |
kityann | 0:1fcc61be1dcf | 71 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 72 | // |
kityann | 0:1fcc61be1dcf | 73 | //Kalman Processing |
kityann | 0:1fcc61be1dcf | 74 | // |
kityann | 0:1fcc61be1dcf | 75 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 76 | void calc_Kalman(){ |
kityann | 0:1fcc61be1dcf | 77 | //calc Kalman gain |
kityann | 0:1fcc61be1dcf | 78 | Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q); |
kityann | 0:1fcc61be1dcf | 79 | Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q); |
kityann | 0:1fcc61be1dcf | 80 | //estimate |
kityann | 0:1fcc61be1dcf | 81 | x_cur = x_prev + Kx*(zx-x_prev); |
kityann | 0:1fcc61be1dcf | 82 | y_cur = y_prev + Ky*(zy-y_prev); |
kityann | 0:1fcc61be1dcf | 83 | //calc sigma |
kityann | 0:1fcc61be1dcf | 84 | s2x_cur = (1-Kx)*(s2x_prev+s2_Q); |
kityann | 0:1fcc61be1dcf | 85 | s2y_cur = (1-Ky)*(s2y_prev+s2_Q); |
kityann | 0:1fcc61be1dcf | 86 | |
kityann | 0:1fcc61be1dcf | 87 | } |
kityann | 0:1fcc61be1dcf | 88 | |
kityann | 0:1fcc61be1dcf | 89 | void Kalman(double Latitude,double Longitude){ |
kityann | 0:1fcc61be1dcf | 90 | |
kityann | 0:1fcc61be1dcf | 91 | zx = Latitude; |
kityann | 0:1fcc61be1dcf | 92 | zy = Longitude; |
kityann | 0:1fcc61be1dcf | 93 | |
kityann | 0:1fcc61be1dcf | 94 | calc_Kalman(); |
kityann | 0:1fcc61be1dcf | 95 | |
kityann | 0:1fcc61be1dcf | 96 | //更新 |
kityann | 0:1fcc61be1dcf | 97 | x_prev = x_cur; |
kityann | 0:1fcc61be1dcf | 98 | y_prev = y_cur; |
kityann | 0:1fcc61be1dcf | 99 | s2x_prev = s2x_cur; |
kityann | 0:1fcc61be1dcf | 100 | s2y_prev = s2y_cur; |
kityann | 0:1fcc61be1dcf | 101 | |
kityann | 3:0bd9ad37f319 | 102 | //robotK\x,robotK_yに格納する |
kityann | 3:0bd9ad37f319 | 103 | cansat.set_robotKalman_x(x_cur); |
kityann | 3:0bd9ad37f319 | 104 | cansat.set_robotKalman_y(y_cur); |
kityann | 0:1fcc61be1dcf | 105 | } |
kityann | 0:1fcc61be1dcf | 106 | |
kityann | 0:1fcc61be1dcf | 107 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 108 | // |
kityann | 0:1fcc61be1dcf | 109 | //Get GPS function |
kityann | 0:1fcc61be1dcf | 110 | // |
kityann | 0:1fcc61be1dcf | 111 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 112 | |
kityann | 0:1fcc61be1dcf | 113 | void Get_GPS(Adafruit_GPS *myGPS){ |
kityann | 0:1fcc61be1dcf | 114 | static int flag = 0; |
kityann | 0:1fcc61be1dcf | 115 | |
kityann | 0:1fcc61be1dcf | 116 | if (myGPS->fix) { |
kityann | 0:1fcc61be1dcf | 117 | |
kityann | 3:0bd9ad37f319 | 118 | cansat.nowStatus = GPS_AVAIL; |
kityann | 3:0bd9ad37f319 | 119 | cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0)); |
kityann | 3:0bd9ad37f319 | 120 | cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0)); |
kityann | 3:0bd9ad37f319 | 121 | |
kityann | 0:1fcc61be1dcf | 122 | |
kityann | 0:1fcc61be1dcf | 123 | if(flag < COUNTER_MAX){ |
kityann | 0:1fcc61be1dcf | 124 | flag++; |
kityann | 0:1fcc61be1dcf | 125 | } |
kityann | 0:1fcc61be1dcf | 126 | if(flag == 5){ |
kityann | 3:0bd9ad37f319 | 127 | x_prev = cansat.get_robot_x(); |
kityann | 3:0bd9ad37f319 | 128 | y_prev = cansat.get_robot_y(); |
kityann | 0:1fcc61be1dcf | 129 | } |
kityann | 0:1fcc61be1dcf | 130 | |
kityann | 0:1fcc61be1dcf | 131 | if(flag >= 6){ |
kityann | 3:0bd9ad37f319 | 132 | if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){ |
kityann | 3:0bd9ad37f319 | 133 | Kalman(cansat.get_robot_x(), cansat.get_robot_y()); |
kityann | 0:1fcc61be1dcf | 134 | change = 1; |
kityann | 0:1fcc61be1dcf | 135 | } |
kityann | 0:1fcc61be1dcf | 136 | else{ |
kityann | 0:1fcc61be1dcf | 137 | change = 0; |
kityann | 0:1fcc61be1dcf | 138 | } |
kityann | 1:f1f7413ae6bd | 139 | //printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n", |
kityann | 1:f1f7413ae6bd | 140 | //agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), |
kityann | 1:f1f7413ae6bd | 141 | //agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), |
kityann | 1:f1f7413ae6bd | 142 | //agz.get_agzCov_lati(),agz.get_agzCov_longi()); |
kityann | 1:f1f7413ae6bd | 143 | } |
kityann | 0:1fcc61be1dcf | 144 | } |
kityann | 3:0bd9ad37f319 | 145 | else cansat.nowStatus = GPS_UNAVAIL; |
kityann | 0:1fcc61be1dcf | 146 | |
kityann | 0:1fcc61be1dcf | 147 | } |
kityann | 0:1fcc61be1dcf | 148 | |
aoki0731 | 4:0fc7221e2f79 | 149 | //ロボットの動き方 |
aoki0731 | 4:0fc7221e2f79 | 150 | char robot_Action(double robot_angle, double target_angle) { |
aoki0731 | 4:0fc7221e2f79 | 151 | |
aoki0731 | 4:0fc7221e2f79 | 152 | double n, t, r; |
aoki0731 | 4:0fc7221e2f79 | 153 | t = target_angle; |
aoki0731 | 4:0fc7221e2f79 | 154 | r = robot_angle; |
aoki0731 | 4:0fc7221e2f79 | 155 | n = r - t; |
aoki0731 | 4:0fc7221e2f79 | 156 | if(n<0) n *= -1; |
aoki0731 | 4:0fc7221e2f79 | 157 | |
aoki0731 | 4:0fc7221e2f79 | 158 | |
aoki0731 | 4:0fc7221e2f79 | 159 | if(t==r) { |
aoki0731 | 4:0fc7221e2f79 | 160 | //前進 |
aoki0731 | 4:0fc7221e2f79 | 161 | return 'f'; |
aoki0731 | 4:0fc7221e2f79 | 162 | } else if(n < 4) { |
aoki0731 | 4:0fc7221e2f79 | 163 | if(t > r) |
aoki0731 | 4:0fc7221e2f79 | 164 | t -= 8; |
aoki0731 | 4:0fc7221e2f79 | 165 | else |
aoki0731 | 4:0fc7221e2f79 | 166 | r -= 8; |
aoki0731 | 4:0fc7221e2f79 | 167 | |
aoki0731 | 4:0fc7221e2f79 | 168 | if(r-t > 0) |
aoki0731 | 4:0fc7221e2f79 | 169 | return 'l'; |
aoki0731 | 4:0fc7221e2f79 | 170 | else |
aoki0731 | 4:0fc7221e2f79 | 171 | return 'r'; |
aoki0731 | 4:0fc7221e2f79 | 172 | } else if(n >= 4) { |
aoki0731 | 4:0fc7221e2f79 | 173 | if(t > r) |
aoki0731 | 4:0fc7221e2f79 | 174 | t -= 8; |
aoki0731 | 4:0fc7221e2f79 | 175 | else |
aoki0731 | 4:0fc7221e2f79 | 176 | r -= 8; |
aoki0731 | 4:0fc7221e2f79 | 177 | |
aoki0731 | 4:0fc7221e2f79 | 178 | if(r-t > 0) |
aoki0731 | 4:0fc7221e2f79 | 179 | return 'r'; |
aoki0731 | 4:0fc7221e2f79 | 180 | else |
aoki0731 | 4:0fc7221e2f79 | 181 | return 'l'; |
aoki0731 | 4:0fc7221e2f79 | 182 | } |
aoki0731 | 4:0fc7221e2f79 | 183 | |
aoki0731 | 4:0fc7221e2f79 | 184 | return 'b'; |
aoki0731 | 4:0fc7221e2f79 | 185 | } |
aoki0731 | 4:0fc7221e2f79 | 186 | |
kityann | 0:1fcc61be1dcf | 187 | /****************************** |
kityann | 0:1fcc61be1dcf | 188 | スタンバイモード |
kityann | 0:1fcc61be1dcf | 189 | ******************************/ |
kityann | 0:1fcc61be1dcf | 190 | void standby(){ |
kityann | 0:1fcc61be1dcf | 191 | ; |
kityann | 0:1fcc61be1dcf | 192 | } |
kityann | 0:1fcc61be1dcf | 193 | |
kityann | 0:1fcc61be1dcf | 194 | /****************************** |
kityann | 0:1fcc61be1dcf | 195 | 落下モード |
kityann | 0:1fcc61be1dcf | 196 | ******************************/ |
kityann | 0:1fcc61be1dcf | 197 | void falling(){ |
kityann | 0:1fcc61be1dcf | 198 | |
kityann | 0:1fcc61be1dcf | 199 | } |
kityann | 0:1fcc61be1dcf | 200 | |
kityann | 0:1fcc61be1dcf | 201 | /****************************** |
kityann | 0:1fcc61be1dcf | 202 | 走行モード |
kityann | 0:1fcc61be1dcf | 203 | ******************************/ |
kityann | 0:1fcc61be1dcf | 204 | void running(){ |
aoki0731 | 4:0fc7221e2f79 | 205 | double r = 6378.137; |
aoki0731 | 4:0fc7221e2f79 | 206 | double y1 = get_ta |
aoki0731 | 4:0fc7221e2f79 | 207 | double y2 = |
aoki0731 | 4:0fc7221e2f79 | 208 | double x1 = |
aoki0731 | 4:0fc7221e2f79 | 209 | double x2 = |
aoki0731 | 4:0fc7221e2f79 | 210 | set_target_distance(r*acos(sin); |
aoki0731 | 4:0fc7221e2f79 | 211 | if(get_compass_z < 0) { |
aoki0731 | 4:0fc7221e2f79 | 212 | //ひっくり返っている |
aoki0731 | 4:0fc7221e2f79 | 213 | } |
kityann | 0:1fcc61be1dcf | 214 | } |
kityann | 0:1fcc61be1dcf | 215 | |
kityann | 0:1fcc61be1dcf | 216 | /****************************** |
kityann | 0:1fcc61be1dcf | 217 | 停止モード |
kityann | 0:1fcc61be1dcf | 218 | ******************************/ |
kityann | 0:1fcc61be1dcf | 219 | void stopping(){ |
kityann | 0:1fcc61be1dcf | 220 | |
kityann | 0:1fcc61be1dcf | 221 | |
kityann | 0:1fcc61be1dcf | 222 | } |
kityann | 0:1fcc61be1dcf | 223 | |
kityann | 0:1fcc61be1dcf | 224 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 225 | // |
kityann | 0:1fcc61be1dcf | 226 | //Main Processing |
kityann | 0:1fcc61be1dcf | 227 | // |
kityann | 0:1fcc61be1dcf | 228 | ///////////////////////////////////////// |
kityann | 0:1fcc61be1dcf | 229 | int main() { |
kityann | 0:1fcc61be1dcf | 230 | //start up time |
kityann | 0:1fcc61be1dcf | 231 | wait(3); |
kityann | 0:1fcc61be1dcf | 232 | //set pc frequency to 57600bps |
kityann | 0:1fcc61be1dcf | 233 | pc.baud(PC_BAUD_RATE); |
kityann | 0:1fcc61be1dcf | 234 | //set xbee frequency to 57600bps |
kityann | 0:1fcc61be1dcf | 235 | xbee.begin(XBEE_BAUD_RATE); |
kityann | 0:1fcc61be1dcf | 236 | |
kityann | 0:1fcc61be1dcf | 237 | |
kityann | 0:1fcc61be1dcf | 238 | //GPS setting |
kityann | 0:1fcc61be1dcf | 239 | gps_Serial = new Serial(p28,p27); |
kityann | 0:1fcc61be1dcf | 240 | Adafruit_GPS myGPS(gps_Serial); |
kityann | 0:1fcc61be1dcf | 241 | Timer refresh_Timer; |
kityann | 0:1fcc61be1dcf | 242 | const int refresh_Time = 1000; //refresh time in ms |
kityann | 0:1fcc61be1dcf | 243 | int count = 0; |
kityann | 0:1fcc61be1dcf | 244 | |
kityann | 0:1fcc61be1dcf | 245 | myGPS.begin(GPS_BAUD_RATE); |
kityann | 0:1fcc61be1dcf | 246 | |
kityann | 0:1fcc61be1dcf | 247 | //GPS Send Command |
kityann | 0:1fcc61be1dcf | 248 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); |
kityann | 0:1fcc61be1dcf | 249 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
kityann | 0:1fcc61be1dcf | 250 | myGPS.sendCommand(PGCMD_ANTENNA); |
kityann | 0:1fcc61be1dcf | 251 | |
kityann | 0:1fcc61be1dcf | 252 | wait_ms(2000); |
kityann | 0:1fcc61be1dcf | 253 | |
kityann | 0:1fcc61be1dcf | 254 | //interrupt start |
kityann | 0:1fcc61be1dcf | 255 | refresh_Timer.start(); |
kityann | 0:1fcc61be1dcf | 256 | |
kityann | 0:1fcc61be1dcf | 257 | printf("start\n"); |
kityann | 0:1fcc61be1dcf | 258 | |
kityann | 0:1fcc61be1dcf | 259 | int mode = -1; |
kityann | 0:1fcc61be1dcf | 260 | |
kityann | 0:1fcc61be1dcf | 261 | while (true) { |
kityann | 0:1fcc61be1dcf | 262 | |
kityann | 0:1fcc61be1dcf | 263 | switch(mode){ |
kityann | 0:1fcc61be1dcf | 264 | //スタートモード:パラシュートが開くまではこのモードを実行 |
kityann | 0:1fcc61be1dcf | 265 | case -1: |
kityann | 0:1fcc61be1dcf | 266 | standby(); |
kityann | 0:1fcc61be1dcf | 267 | break; |
kityann | 0:1fcc61be1dcf | 268 | //落下モード:落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する |
kityann | 0:1fcc61be1dcf | 269 | case 1: |
kityann | 0:1fcc61be1dcf | 270 | falling(); |
kityann | 0:1fcc61be1dcf | 271 | break; |
kityann | 0:1fcc61be1dcf | 272 | //走行モード:ターゲットにむかって走行を行う |
kityann | 0:1fcc61be1dcf | 273 | case 2: |
kityann | 0:1fcc61be1dcf | 274 | running(); |
kityann | 0:1fcc61be1dcf | 275 | break; |
kityann | 0:1fcc61be1dcf | 276 | //停止モード:ターゲット |
kityann | 0:1fcc61be1dcf | 277 | case 100: |
kityann | 0:1fcc61be1dcf | 278 | stopping(); |
kityann | 0:1fcc61be1dcf | 279 | break; |
kityann | 0:1fcc61be1dcf | 280 | } |
kityann | 0:1fcc61be1dcf | 281 | |
kityann | 0:1fcc61be1dcf | 282 | myGPS.read(); |
kityann | 0:1fcc61be1dcf | 283 | //recive gps module |
kityann | 0:1fcc61be1dcf | 284 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
kityann | 0:1fcc61be1dcf | 285 | if ( myGPS.newNMEAreceived() ) { |
kityann | 0:1fcc61be1dcf | 286 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
kityann | 0:1fcc61be1dcf | 287 | continue; |
kityann | 0:1fcc61be1dcf | 288 | } |
kityann | 0:1fcc61be1dcf | 289 | else{ |
kityann | 0:1fcc61be1dcf | 290 | count++; |
kityann | 0:1fcc61be1dcf | 291 | } |
kityann | 0:1fcc61be1dcf | 292 | } |
kityann | 0:1fcc61be1dcf | 293 | //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する |
kityann | 0:1fcc61be1dcf | 294 | if (refresh_Timer.read_ms() >= refresh_Time) { |
kityann | 0:1fcc61be1dcf | 295 | refresh_Timer.reset(); |
kityann | 0:1fcc61be1dcf | 296 | //print_gps(count); |
kityann | 0:1fcc61be1dcf | 297 | Get_GPS(&myGPS); |
kityann | 0:1fcc61be1dcf | 298 | |
kityann | 0:1fcc61be1dcf | 299 | } |
kityann | 0:1fcc61be1dcf | 300 | } |
kityann | 0:1fcc61be1dcf | 301 | |
kityann | 0:1fcc61be1dcf | 302 | } |