aigamozu / Mbed 2 deprecated aigamozu_program_ver2_yokokawa

yokokawa

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

Fork of aigamozu_program_ver2 by aigamozu

main.cpp@0:7d55d6ace996, 2014-05-20 (annotated)

Committer:
m5171135
Date:
Tue May 20 14:51:25 2014 +0000
Revision:
0:7d55d6ace996
Child:
1:490b793b2e61
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m5171135 0:7d55d6ace996 1 #include "mbed.h"
m5171135 0:7d55d6ace996 2 #include "XBee.h"
m5171135 0:7d55d6ace996 3 #include "MBed_Adafruit_GPS.h"
m5171135 0:7d55d6ace996 4 /////////////////////////////////////////
m5171135 0:7d55d6ace996 5 //
m5171135 0:7d55d6ace996 6 //Connection Setting
m5171135 0:7d55d6ace996 7 //
m5171135 0:7d55d6ace996 8 /////////////////////////////////////////
m5171135 0:7d55d6ace996 9 DigitalOut led1(LED1);
m5171135 0:7d55d6ace996 10 //IMU -> i2c
m5171135 0:7d55d6ace996 11 I2C i2c(p9, p10);
m5171135 0:7d55d6ace996 12 //set serial
m5171135 0:7d55d6ace996 13 Serial pc(USBTX, USBRX); //tx, rx
m5171135 0:7d55d6ace996 14 XBee xbee(p13,p14); //tx,rx
m5171135 0:7d55d6ace996 15 Serial * gps_Serial;
m5171135 0:7d55d6ace996 16 //gps responce
m5171135 0:7d55d6ace996 17 ZBRxResponse zbRx = ZBRxResponse();
m5171135 0:7d55d6ace996 18 XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67);
m5171135 0:7d55d6ace996 19
m5171135 0:7d55d6ace996 20
m5171135 0:7d55d6ace996 21 /////////////////////////////////////////
m5171135 0:7d55d6ace996 22 //
m5171135 0:7d55d6ace996 23 //Pin Setting
m5171135 0:7d55d6ace996 24 //
m5171135 0:7d55d6ace996 25 /////////////////////////////////////////
m5171135 0:7d55d6ace996 26
m5171135 0:7d55d6ace996 27 //Motor & Encorder Setting
m5171135 0:7d55d6ace996 28 //Left
m5171135 0:7d55d6ace996 29 DigitalOut motorL_A(p21);
m5171135 0:7d55d6ace996 30 DigitalOut motorL_B(p22);
m5171135 0:7d55d6ace996 31 PwmOut motorL_pwm(p23);
m5171135 0:7d55d6ace996 32 InterruptIn encoderR_A(p15);
m5171135 0:7d55d6ace996 33 InterruptIn encoderR_B(p16);
m5171135 0:7d55d6ace996 34
m5171135 0:7d55d6ace996 35 //Rignt
m5171135 0:7d55d6ace996 36 DigitalOut motorR_A(p24);
m5171135 0:7d55d6ace996 37 DigitalOut motorR_B(p25);
m5171135 0:7d55d6ace996 38 PwmOut motorR_pwm(p26);
m5171135 0:7d55d6ace996 39
m5171135 0:7d55d6ace996 40 //test data
m5171135 0:7d55d6ace996 41 Ticker axis;
m5171135 0:7d55d6ace996 42 Ticker auth_axis;
m5171135 0:7d55d6ace996 43
m5171135 0:7d55d6ace996 44 /////////////////////////////////////////
m5171135 0:7d55d6ace996 45 //
m5171135 0:7d55d6ace996 46 //Each Value Setting
m5171135 0:7d55d6ace996 47 //
m5171135 0:7d55d6ace996 48 /////////////////////////////////////////
m5171135 0:7d55d6ace996 49 //my status
m5171135 0:7d55d6ace996 50 //0: StndbyMode
m5171135 0:7d55d6ace996 51 //1: ManualMode
m5171135 0:7d55d6ace996 52 //2: AuthmaticMode(Random)
m5171135 0:7d55d6ace996 53 unsigned char my_status = 0;
m5171135 0:7d55d6ace996 54
m5171135 0:7d55d6ace996 55 //my_status
m5171135 0:7d55d6ace996 56 // 0 -> stable
m5171135 0:7d55d6ace996 57 // 1 -> error
m5171135 0:7d55d6ace996 58
m5171135 0:7d55d6ace996 59 //0 bit: Motor Satatus
m5171135 0:7d55d6ace996 60 //1 bit: GPS Status
m5171135 0:7d55d6ace996 61 //2 bit: Sensor Status
m5171135 0:7d55d6ace996 62 //3 bit: Battery Status
m5171135 0:7d55d6ace996 63 //4 bit:
m5171135 0:7d55d6ace996 64 //5 bit:
m5171135 0:7d55d6ace996 65 //6 bit:
m5171135 0:7d55d6ace996 66 //7 bit:
m5171135 0:7d55d6ace996 67
m5171135 0:7d55d6ace996 68 unsigned char my_mode = 0;
m5171135 0:7d55d6ace996 69
m5171135 0:7d55d6ace996 70 //I2C address 9-axis
m5171135 0:7d55d6ace996 71 const int gyro_addr = 0xD0;
m5171135 0:7d55d6ace996 72 const int acc_addr = 0xA6;
m5171135 0:7d55d6ace996 73
m5171135 0:7d55d6ace996 74 char gyro_head[1];
m5171135 0:7d55d6ace996 75 char read[8];
m5171135 0:7d55d6ace996 76 short int gyro_x=0;
m5171135 0:7d55d6ace996 77 short int gyro_y=0;
m5171135 0:7d55d6ace996 78 short int gyro_z=0;
m5171135 0:7d55d6ace996 79 short int tempr=0;
m5171135 0:7d55d6ace996 80
m5171135 0:7d55d6ace996 81 char acc_head[1];
m5171135 0:7d55d6ace996 82 char acc_buf[6];
m5171135 0:7d55d6ace996 83 short int acc_x = 0;
m5171135 0:7d55d6ace996 84 short int acc_y = 0;
m5171135 0:7d55d6ace996 85 short int acc_z = 0;
m5171135 0:7d55d6ace996 86
m5171135 0:7d55d6ace996 87 //GPS value
m5171135 0:7d55d6ace996 88 //float longitude = 0.0;
m5171135 0:7d55d6ace996 89 //float latitude = 0.0;
m5171135 0:7d55d6ace996 90
m5171135 0:7d55d6ace996 91 //motor_speed_feed_back
m5171135 0:7d55d6ace996 92 float target_palse = 0.0;
m5171135 0:7d55d6ace996 93 float pwm;
m5171135 0:7d55d6ace996 94 long encorder_count = 0;
m5171135 0:7d55d6ace996 95 int count = 0;
m5171135 0:7d55d6ace996 96
m5171135 0:7d55d6ace996 97 //ManualValue
m5171135 0:7d55d6ace996 98 int manual_count = 0;
m5171135 0:7d55d6ace996 99 int manual_flag = 0;
m5171135 0:7d55d6ace996 100
m5171135 0:7d55d6ace996 101
m5171135 0:7d55d6ace996 102 //test value
m5171135 0:7d55d6ace996 103 long sub_latH = 12345;
m5171135 0:7d55d6ace996 104 long sub_latL = 67890;
m5171135 0:7d55d6ace996 105
m5171135 0:7d55d6ace996 106 long sub_lonH = 98765;
m5171135 0:7d55d6ace996 107 long sub_lonL = 43211;
m5171135 0:7d55d6ace996 108
m5171135 0:7d55d6ace996 109
m5171135 0:7d55d6ace996 110 union UNI_TEST_T
m5171135 0:7d55d6ace996 111 {
m5171135 0:7d55d6ace996 112 long a;
m5171135 0:7d55d6ace996 113 uint8_t b[4];
m5171135 0:7d55d6ace996 114 };
m5171135 0:7d55d6ace996 115
m5171135 0:7d55d6ace996 116
m5171135 0:7d55d6ace996 117
m5171135 0:7d55d6ace996 118
m5171135 0:7d55d6ace996 119 UNI_TEST_T latH,latL,lonH,lonL;
m5171135 0:7d55d6ace996 120
m5171135 0:7d55d6ace996 121
m5171135 0:7d55d6ace996 122
m5171135 0:7d55d6ace996 123
m5171135 0:7d55d6ace996 124
m5171135 0:7d55d6ace996 125 /////////////////////////////////////////
m5171135 0:7d55d6ace996 126 //
m5171135 0:7d55d6ace996 127 //Change Motor State Processing
m5171135 0:7d55d6ace996 128 //
m5171135 0:7d55d6ace996 129 /////////////////////////////////////////
m5171135 0:7d55d6ace996 130 void change_speed(unsigned char L_state,unsigned char L_pwm,unsigned char R_state,unsigned char R_pwm){
m5171135 0:7d55d6ace996 131 switch(L_state){
m5171135 0:7d55d6ace996 132 case 0:
m5171135 0:7d55d6ace996 133 motorL_A = 0;
m5171135 0:7d55d6ace996 134 motorL_B = 0;
m5171135 0:7d55d6ace996 135 break;
m5171135 0:7d55d6ace996 136
m5171135 0:7d55d6ace996 137 case 1:
m5171135 0:7d55d6ace996 138 motorL_A = 1;
m5171135 0:7d55d6ace996 139 motorL_B = 0;
m5171135 0:7d55d6ace996 140 break;
m5171135 0:7d55d6ace996 141
m5171135 0:7d55d6ace996 142 case 2:
m5171135 0:7d55d6ace996 143 motorL_A = 0;
m5171135 0:7d55d6ace996 144 motorL_B = 1;
m5171135 0:7d55d6ace996 145 break;
m5171135 0:7d55d6ace996 146
m5171135 0:7d55d6ace996 147 default:
m5171135 0:7d55d6ace996 148 motorL_A = 0;
m5171135 0:7d55d6ace996 149 motorL_B = 0;
m5171135 0:7d55d6ace996 150 break;
m5171135 0:7d55d6ace996 151 }
m5171135 0:7d55d6ace996 152
m5171135 0:7d55d6ace996 153 motorL_pwm = (float)L_pwm/256.0;
m5171135 0:7d55d6ace996 154
m5171135 0:7d55d6ace996 155 switch(R_state){
m5171135 0:7d55d6ace996 156 case 0:
m5171135 0:7d55d6ace996 157 motorR_A = 0;
m5171135 0:7d55d6ace996 158 motorR_B = 0;
m5171135 0:7d55d6ace996 159 break;
m5171135 0:7d55d6ace996 160
m5171135 0:7d55d6ace996 161 case 1:
m5171135 0:7d55d6ace996 162 motorR_A = 1;
m5171135 0:7d55d6ace996 163 motorR_B = 0;
m5171135 0:7d55d6ace996 164 break;
m5171135 0:7d55d6ace996 165
m5171135 0:7d55d6ace996 166 case 2:
m5171135 0:7d55d6ace996 167 motorR_A = 0;
m5171135 0:7d55d6ace996 168 motorR_B = 1;
m5171135 0:7d55d6ace996 169 break;
m5171135 0:7d55d6ace996 170
m5171135 0:7d55d6ace996 171 default:
m5171135 0:7d55d6ace996 172 motorR_A = 0;
m5171135 0:7d55d6ace996 173 motorR_B = 0;
m5171135 0:7d55d6ace996 174 break;
m5171135 0:7d55d6ace996 175 }
m5171135 0:7d55d6ace996 176 motorR_pwm = (float)R_pwm/256.0;
m5171135 0:7d55d6ace996 177
m5171135 0:7d55d6ace996 178 }
m5171135 0:7d55d6ace996 179
m5171135 0:7d55d6ace996 180 /////////////////////////////////////////
m5171135 0:7d55d6ace996 181 //
m5171135 0:7d55d6ace996 182 //Interruption processing 1: time -> 0.1s
m5171135 0:7d55d6ace996 183 //
m5171135 0:7d55d6ace996 184 /////////////////////////////////////////
m5171135 0:7d55d6ace996 185 void axisRenovation(){
m5171135 0:7d55d6ace996 186 //gyro
m5171135 0:7d55d6ace996 187 gyro_head[0] = 0x1B;
m5171135 0:7d55d6ace996 188 i2c.write(gyro_addr, gyro_head, 1);
m5171135 0:7d55d6ace996 189 i2c.read(gyro_addr, read, 8);
m5171135 0:7d55d6ace996 190
m5171135 0:7d55d6ace996 191 tempr=(read[0] << 8) + read[1];
m5171135 0:7d55d6ace996 192 gyro_x=(read[2] << 8) + read[3];
m5171135 0:7d55d6ace996 193 gyro_y=(read[4] << 8) + read[5];
m5171135 0:7d55d6ace996 194 gyro_z=(read[6] << 8) + read[7];
m5171135 0:7d55d6ace996 195
m5171135 0:7d55d6ace996 196 //acc
m5171135 0:7d55d6ace996 197 acc_head[0] = 0x32;
m5171135 0:7d55d6ace996 198 i2c.write(acc_addr,acc_head,1);
m5171135 0:7d55d6ace996 199 i2c.read(acc_addr, acc_buf, 6);
m5171135 0:7d55d6ace996 200
m5171135 0:7d55d6ace996 201 acc_x = (acc_buf[1] << 8) + acc_buf[0];
m5171135 0:7d55d6ace996 202 acc_y = (acc_buf[3] << 8) + acc_buf[2];
m5171135 0:7d55d6ace996 203 acc_z = (acc_buf[5] << 8) + acc_buf[4];
m5171135 0:7d55d6ace996 204
m5171135 0:7d55d6ace996 205
m5171135 0:7d55d6ace996 206 if(manual_count > 100){
m5171135 0:7d55d6ace996 207 change_speed(0,0,0,0);
m5171135 0:7d55d6ace996 208 manual_count = 0;
m5171135 0:7d55d6ace996 209 }
m5171135 0:7d55d6ace996 210
m5171135 0:7d55d6ace996 211 manual_count++;
m5171135 0:7d55d6ace996 212
m5171135 0:7d55d6ace996 213 //printf("Mode -> %d\n", my_mode);
m5171135 0:7d55d6ace996 214 //printf("Status -> %d\n", my_status);
m5171135 0:7d55d6ace996 215 //printf("Status -> %d\n", count);
m5171135 0:7d55d6ace996 216
m5171135 0:7d55d6ace996 217 }
m5171135 0:7d55d6ace996 218
m5171135 0:7d55d6ace996 219 /////////////////////////////////////////
m5171135 0:7d55d6ace996 220 //
m5171135 0:7d55d6ace996 221 //Interruption processing: time -> 1.0s
m5171135 0:7d55d6ace996 222 //
m5171135 0:7d55d6ace996 223 /////////////////////////////////////////
m5171135 0:7d55d6ace996 224
m5171135 0:7d55d6ace996 225 void randomRenovation(){
m5171135 0:7d55d6ace996 226
m5171135 0:7d55d6ace996 227 if(count < 30){
m5171135 0:7d55d6ace996 228 change_speed(1,127,1,127);
m5171135 0:7d55d6ace996 229 }
m5171135 0:7d55d6ace996 230
m5171135 0:7d55d6ace996 231 else{
m5171135 0:7d55d6ace996 232 change_speed(1,127,2,127);
m5171135 0:7d55d6ace996 233 if(count > 35) count = 0;
m5171135 0:7d55d6ace996 234 }
m5171135 0:7d55d6ace996 235 count++;
m5171135 0:7d55d6ace996 236 }
m5171135 0:7d55d6ace996 237
m5171135 0:7d55d6ace996 238
m5171135 0:7d55d6ace996 239
m5171135 0:7d55d6ace996 240 /////////////////////////////////////////
m5171135 0:7d55d6ace996 241 //
m5171135 0:7d55d6ace996 242 //Main Processing
m5171135 0:7d55d6ace996 243 //
m5171135 0:7d55d6ace996 244 /////////////////////////////////////////
m5171135 0:7d55d6ace996 245 int main() {
m5171135 0:7d55d6ace996 246 //start up time
m5171135 0:7d55d6ace996 247 wait(3);
m5171135 0:7d55d6ace996 248 //set i2c frequency to 400 KHz
m5171135 0:7d55d6ace996 249 i2c.frequency(400000);
m5171135 0:7d55d6ace996 250 //set pc frequency to 57600bps
m5171135 0:7d55d6ace996 251 pc.baud(57600);
m5171135 0:7d55d6ace996 252 //set xbee frequency to 57600bps
m5171135 0:7d55d6ace996 253 xbee.begin(57600);
m5171135 0:7d55d6ace996 254
m5171135 0:7d55d6ace996 255 //GPS setting
m5171135 0:7d55d6ace996 256 gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
m5171135 0:7d55d6ace996 257 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
m5171135 0:7d55d6ace996 258 //char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
m5171135 0:7d55d6ace996 259 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
m5171135 0:7d55d6ace996 260 const int refresh_Time = 2000; //refresh time in ms
m5171135 0:7d55d6ace996 261
m5171135 0:7d55d6ace996 262 myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
m5171135 0:7d55d6ace996 263 //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
m5171135 0:7d55d6ace996 264
m5171135 0:7d55d6ace996 265 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
m5171135 0:7d55d6ace996 266 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
m5171135 0:7d55d6ace996 267 myGPS.sendCommand(PGCMD_ANTENNA);
m5171135 0:7d55d6ace996 268
m5171135 0:7d55d6ace996 269 //gyro_registor Setting
m5171135 0:7d55d6ace996 270 char PWR_M[2]={0x3E,0x80};
m5171135 0:7d55d6ace996 271 i2c.write(gyro_addr, PWR_M, 2); // Send command string
m5171135 0:7d55d6ace996 272 char SMPL[2]={0x15,0x00};
m5171135 0:7d55d6ace996 273 i2c.write(gyro_addr, SMPL, 2); // Send command string
m5171135 0:7d55d6ace996 274 char DLPF[2]={0x16,0x18};
m5171135 0:7d55d6ace996 275 i2c.write(gyro_addr, DLPF, 2); // Send command string
m5171135 0:7d55d6ace996 276 char INT_C[2]={0x17,0x05};
m5171135 0:7d55d6ace996 277 i2c.write(gyro_addr, INT_C, 2); // Send commanad string
m5171135 0:7d55d6ace996 278 char PWR_M2[2]={0x3E,0x00};
m5171135 0:7d55d6ace996 279 i2c.write(gyro_addr, PWR_M2, 2); // Send command string
m5171135 0:7d55d6ace996 280
m5171135 0:7d55d6ace996 281 wait(2);
m5171135 0:7d55d6ace996 282
m5171135 0:7d55d6ace996 283 //interrupt start
m5171135 0:7d55d6ace996 284 axis.attach(&axisRenovation, 0.1);
m5171135 0:7d55d6ace996 285 motorL_pwm = 0.0;
m5171135 0:7d55d6ace996 286 motorR_pwm = 0.0;
m5171135 0:7d55d6ace996 287
m5171135 0:7d55d6ace996 288 refresh_Timer.start();
m5171135 0:7d55d6ace996 289
m5171135 0:7d55d6ace996 290 while (1) {
m5171135 0:7d55d6ace996 291
m5171135 0:7d55d6ace996 292 //recive xbee module
m5171135 0:7d55d6ace996 293 xbee.readPacket();
m5171135 0:7d55d6ace996 294 if (xbee.getResponse().isAvailable()) {
m5171135 0:7d55d6ace996 295 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
m5171135 0:7d55d6ace996 296 xbee.getResponse().getZBRxResponse(zbRx);
m5171135 0:7d55d6ace996 297 unsigned char *buf = zbRx.getFrameData();
m5171135 0:7d55d6ace996 298
m5171135 0:7d55d6ace996 299 switch((unsigned char)buf[14]){
m5171135 0:7d55d6ace996 300
m5171135 0:7d55d6ace996 301 //ChanegeMode
m5171135 0:7d55d6ace996 302 case 'C':{
m5171135 0:7d55d6ace996 303 my_mode = buf[19];
m5171135 0:7d55d6ace996 304 change_speed(0,0,0,0);
m5171135 0:7d55d6ace996 305
m5171135 0:7d55d6ace996 306 if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0);
m5171135 0:7d55d6ace996 307 else auth_axis.detach();
m5171135 0:7d55d6ace996 308 break;
m5171135 0:7d55d6ace996 309 }
m5171135 0:7d55d6ace996 310
m5171135 0:7d55d6ace996 311 //MunualCommand
m5171135 0:7d55d6ace996 312 case 'M':{
m5171135 0:7d55d6ace996 313 if(my_mode == 1){
m5171135 0:7d55d6ace996 314 manual_count = 0;
m5171135 0:7d55d6ace996 315 change_speed(buf[20],buf[21],buf[23],buf[24]);
m5171135 0:7d55d6ace996 316
m5171135 0:7d55d6ace996 317 }
m5171135 0:7d55d6ace996 318 break;
m5171135 0:7d55d6ace996 319 }
m5171135 0:7d55d6ace996 320
m5171135 0:7d55d6ace996 321 //SendStatus
m5171135 0:7d55d6ace996 322 case 'S':{
m5171135 0:7d55d6ace996 323 //latH.a = myGPS.latitudeH;;
m5171135 0:7d55d6ace996 324 //latL.a = myGPS.latitudeL;
m5171135 0:7d55d6ace996 325 //lonH.a = myGPS.longitudeH;
m5171135 0:7d55d6ace996 326 //lonL.a = myGPS.longitudeL;
m5171135 0:7d55d6ace996 327 led1 = 1;
m5171135 0:7d55d6ace996 328 //dummy data
m5171135 0:7d55d6ace996 329 latH.a = sub_latH;
m5171135 0:7d55d6ace996 330 latL.a = sub_latL;
m5171135 0:7d55d6ace996 331 lonH.a = sub_lonH;
m5171135 0:7d55d6ace996 332 lonL.a = sub_lonL;
m5171135 0:7d55d6ace996 333
m5171135 0:7d55d6ace996 334 uint8_t data[] = {65,71,83,82,70,65,84,66,83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
m5171135 0:7d55d6ace996 335 ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
m5171135 0:7d55d6ace996 336 xbee.send(tx64request);
m5171135 0:7d55d6ace996 337 break;
m5171135 0:7d55d6ace996 338 }
m5171135 0:7d55d6ace996 339
m5171135 0:7d55d6ace996 340 default:
m5171135 0:7d55d6ace996 341 {
m5171135 0:7d55d6ace996 342 break;
m5171135 0:7d55d6ace996 343 }
m5171135 0:7d55d6ace996 344 }
m5171135 0:7d55d6ace996 345 }
m5171135 0:7d55d6ace996 346 }
m5171135 0:7d55d6ace996 347
m5171135 0:7d55d6ace996 348 myGPS.read();
m5171135 0:7d55d6ace996 349 //recive gps module
m5171135 0:7d55d6ace996 350 //check if we recieved a new message from GPS, if so, attempt to parse it,
m5171135 0:7d55d6ace996 351 if ( myGPS.newNMEAreceived() ) {
m5171135 0:7d55d6ace996 352 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
m5171135 0:7d55d6ace996 353 continue;
m5171135 0:7d55d6ace996 354 }
m5171135 0:7d55d6ace996 355 }
m5171135 0:7d55d6ace996 356
m5171135 0:7d55d6ace996 357 if (refresh_Timer.read_ms() >= refresh_Time) {
m5171135 0:7d55d6ace996 358 refresh_Timer.reset();
m5171135 0:7d55d6ace996 359 if (myGPS.fix) {
m5171135 0:7d55d6ace996 360 my_status = 0;
m5171135 0:7d55d6ace996 361 //longitude = myGPS.longitudeH;
m5171135 0:7d55d6ace996 362 //latitude = myGPS.latitudeH;
m5171135 0:7d55d6ace996 363 }
m5171135 0:7d55d6ace996 364
m5171135 0:7d55d6ace996 365 else my_status = 1;
m5171135 0:7d55d6ace996 366 }
m5171135 0:7d55d6ace996 367 }
m5171135 0:7d55d6ace996 368 }