openXsensor/oXs_gps.cpp

   1 #include "oXs_gps.h"
   2 #include <avr/pgmspace.h>
   3
   4 #ifdef DEBUG
   5 //#define DEBUGGPS
   6 #define DEBUGPARSEGPS
   7 //#define DEBUGSENDGPS
   8 //#define DEBUG_FORWARD_GPS_MSG_TO_PC
   9 #endif
  10
  11 //#define DEBUGSIMULATEGPS
  12
  13 extern unsigned long micros( void ) ;
  14 extern unsigned long millis( void ) ;
  15 extern void delay(unsigned long ms) ;
  16
  17
  18 // **********************
  19 // GPS data being read
  20 // **********************
  21 int32_t GPS_lon;               // longitude in degree with 7 decimals, (neg for S)
  22 bool    GPS_lonAvailable = false ;
  23 int32_t GPS_lat;               // latitude   in degree with 7 decimals, (neg for ?)
  24 bool    GPS_latAvailable = false ;
  25
  26 int32_t GPS_altitude;              // altitude in mm
  27 bool    GPS_altitudeAvailable = false ;
  28 uint16_t GPS_speed_3d;                 // speed in cm/s
  29 bool    GPS_speed_3dAvailable = false;
  30 uint16_t GPS_speed_2d;                 // speed in cm/s
  31 bool    GPS_speed_2dAvailable = false ;
  32 uint32_t GPS_ground_course ;     // degrees with 5 decimals
  33 bool    GPS_ground_courseAvailable = false ;
  34
  35 #if defined(GPS_TRANSMIT_TIME)
  36 int16_t GPS_year;               // year
  37 int8_t GPS_month;               // month
  38 int8_t GPS_day;                 // day
  39 int8_t GPS_hour;                // hours
  40 int8_t GPS_min;                 // minutes
  41 int8_t GPS_sec;                 // seconds
  42 bool    GPS_timeAvailable = false ;
  43 #endif
  44
  45 uint8_t GPS_numSat;
  46 uint8_t GPS_fix_type;
  47 uint16_t GPS_hdop = 9999;           // Compute GPS quality signal
  48 uint16_t GPS_packetCount = 0;
  49 //uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
  50 //uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
  51 //uint8_t GPS_numCh;                          // Number of channels
  52 //uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
  53 //uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
  54 //uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
  55 //uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
  56
  57 // *********** GPS calculated data
  58 int16_t GPS_distance ;   // distance from home (first location) in m
  59 int16_t GPS_heading ;          // heading from home (in Rad)
  60 int32_t GPS_home_lat ;         // position of home in degre with 7 decimals
  61 int32_t GPS_home_lon ;         // position of home in degre with 7 decimals
  62 float GPS_scale ;     // scaling factor to calculate the distance depending on latitude
  63 int16_t GPS_bearing ;          // bearing from home in degrees
  64 // scaling factor from 1e-7 degrees to meters at equater
  65 // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
  66 #define LOCATION_SCALING_FACTOR 0.011131884502145034f
  67 #define DEG_TO_RAD_FOR_GPS 0.017453292519943295769236907684886f
  68
  69 bool GPS_fix = false ; // true if gps data are available.
  70 static uint8_t _msg_id; //used to identify the message type when reading the gps, is used also when buffer is parsed
  71
  72 #if defined( A_LOCATOR_IS_CONNECTED)  && ( A_LOCATOR_IS_CONNECTED == YES)
  73 uint32_t GPS_last_fix_millis ; // time when last fix has been received (used by lora locator)
  74 int32_t GPS_last_fix_lon ;     // last lon when a fix has been received
  75 int32_t GPS_last_fix_lat ;     // last lat when a fix has been received
  76 #endif
  77
  78 // Receive buffer
  79 static union {
  80     ubx_nav_posllh posllh;
  81     ubx_nav_status status;
  82     ubx_nav_solution solution;
  83     ubx_nav_velned velned;
  84 #if defined(GPS_TRANSMIT_TIME)
  85     ubx_nav_timeutc timeutc;
  86 #endif
  87     ubx_nav_svinfo svinfo;
  88     uint8_t bytes[UBLOX_BUFFER_SIZE];
  89 } _buffer;
  90
  91
  92 #ifdef DEBUG
  93 OXS_GPS::OXS_GPS(HardwareSerial &print)
  94 #else
  95 OXS_GPS::OXS_GPS( uint8_t x )
  96 #endif
  97 {
  98   // constructor
  99 #ifdef DEBUG
 100   printer = &print; //operate on the address of print
 101 #endif
 102 }
 103
 104 // **************** Setup the GPS sensor *********************
 105 #define SOFT_TX_BIT 6  // define which port is used to send the set up to the GPS (here pin arduino 6 = Port D6 is used
 106 #define SOFT_TX_DDR DDRD  // specify the port being used for this pin
 107 #define SOFT_TX_PORT PORTD  // specify the port being used for this pin
 108
 109 #define MICROSECONDS_OVERHEAD_ADJUST 0
 110
 111 #define MICROSECONDS_PER_BIT_9600 ((1000000ul / 9600) - MICROSECONDS_OVERHEAD_ADJUST)
 112 #define MICROSECONDS_PER_BIT_38400 ((1000000ul / 38400ul) - MICROSECONDS_OVERHEAD_ADJUST)
 113
 114
 115 void gps_putchar (char c, uint16_t usPerBit) {
 116   uint8_t  bit_mask;
 117   SOFT_TX_PORT &= ~(1<<SOFT_TX_BIT); // start bit
 118   delayMicroseconds(usPerBit);
 119   // data bits
 120   for (bit_mask=0x01; bit_mask; bit_mask<<=1) {
 121     if (c & bit_mask) {
 122       SOFT_TX_PORT |= (1<<SOFT_TX_BIT);  // set output to 1
 123     }
 124     else {
 125       SOFT_TX_PORT &= ~(1<<SOFT_TX_BIT); // set output to 0
 126     }
 127     delayMicroseconds(usPerBit);
 128   }
 129   SOFT_TX_PORT |= (1<<SOFT_TX_BIT);   // 2 stop bits
 130   delayMicroseconds(usPerBit * 2);
 131   return ;
 132 }
 133
 134
 135 // send config commands to GPS at 9600 bds (default baud rate)
 136 void OXS_GPS::setupGps( ) {
 137       const static uint8_t initGps1[] PROGMEM = {
 138 // Here the code to activate galileo sat. (This has not yet been tested and is based on I-NAV code)
 139         0xB5,0x62,0x06,0x3E, 0x3C, 0x00, // GNSS + number of bytes= 60 dec = 003C in HEx
 140         0x00, 0x00, 0x20, 0x07,  // GNSS / min / max / enable
 141         0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y
 142         0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y
 143         0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y
 144         0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N
 145         0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N
 146         0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N
 147         0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS / 8 / 14 / Y
 148         0x30, 0xD8, // checksum
 149
 150 // Here the code to activate SBAS for Europe (This has not yet been tested and is based on I-NAV code)
 151         0xB5,0x62,0x06,0x16, 0x08, 0x00, // SBAS + number of bytes = 8
 152         0x03, 0x03, 0x03, 0x00, // mode = test + enabled, usage=range+diffcorr, max =3, scanmode2=0
 153         0x00, 0x00, 0x08, 0x51, // scanmode1 120,124, 126, 131
 154         0x86, 0x2C, //checksum
 155
 156 // Here other code
 157         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x01,0x00,0x00,0x00,0x00,0x13,0xBE, // activate NAV-POSLLH message
 158         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x06,0x00,0x01,0x00,0x00,0x00,0x00,0x17,0xDA, //        NAV-SOL
 159         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x01,0x00,0x00,0x00,0x00,0x23,0x2E, //        NAV-VELNED
 160 #if defined(GPS_TRANSMIT_TIME)
 161         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x21,0x00,0x01,0x00,0x00,0x00,0x00,0x32,0x97, //........NAV-TIMEUTC
 162 #endif
 163 #if defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 1)
 164         0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39,  // NAV-RATE for 1 hz
 165 #elif defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 10)
 166         0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12, // NAV-RATE for 10 hz
 167 #else
 168         0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A, // NAV-RATE for 5 hz
 169 #endif
 170         0xB5,0x62,0x06,0x00,0x14,0x00,0x01,0x00,0x00,0x00,0xD0,0x08,0x00,0x00,0x00,0x96, //        CFG-PRT : Set port to output only UBX (so deactivate NMEA msg) and set baud = 38400.
 171                             0x00,0x00,0x07,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x91,0x84  //                 rest of CFG_PRT command
 172       }  ;
 173       uint8_t initGpsIdx = 0 ;
 174   SOFT_TX_PORT |= (1<<SOFT_TX_BIT);   // put 1 in output port
 175   SOFT_TX_DDR |= (1<<SOFT_TX_BIT);    // set port as output
 176   delay(3000) ; // wait to be sure that GPS has started
 177   while (initGpsIdx < sizeof( initGps1)) {
 178 //    Serial.println( pgm_read_byte_near(initGps1 + initGpsIdx ), HEX) ;
 179     gps_putchar( pgm_read_byte_near(initGps1 + initGpsIdx++ ), MICROSECONDS_PER_BIT_9600) ; // Send initialisation command
 180     delay(10) ;
 181   }
 182   delay(1000) ;
 183 #ifdef DEBUG
 184   Serial.println(F("End of GPS setup")) ;
 185 #endif
 186 }
 187
 188
 189 // **************** Read the GPS    *********************
 190
 191 void OXS_GPS::readGps() { // read and process GPS data. do not send them.
 192 #ifdef DEBUGSIMULATEGPS
 193       const static uint8_t simulateGpsBuffer[] PROGMEM = { // POS message
 194         0xB5,0x62,0x01,0x02,0x1C,0x00,0x58,0xF2,0x1A,0x14,0xB0,0x4D,0xA1,0xCF,0xAC,0xD5,0x1F,0x1D,0xE0,0x93,0x04,0x00,0xE0,0x93,0x04,0x00,0x06,0x17,0x00,0x00,0x3C,0x24,0x00,0x00,0x2C,0xF5
 195        ,0xB5,0x62,0x01,0x12,0x24,0x00,0x58,0xF2,0x1A,0x14,0xFC,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFC,0x06,0x00,0x00,0xFC,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x11,0x01,0x00,0x00,0x00,0x51,0x25,0x02,0x3F,0x00
 196       }  ;
 197       static uint8_t simuIdx= 0 ;
 198 //      printer->print(simuIdx) ;printer->print(",") ;
 199        gpsNewFrameUBLOX( pgm_read_byte_near(simulateGpsBuffer + simuIdx) );
 200 //  printer->print(F("Gps char: "));
 201 //  printer->println( pgm_read_byte_near(simulateGpsBuffer[simuIdx]),hex );
 202         simuIdx++ ;
 203         if ( simuIdx >= sizeof( simulateGpsBuffer) ) simuIdx = 0  ;
 204 #else
 205        static uint8_t gpsChar ;
 206        while  (Serial.available() ) { // read all bytes in Serial Rx buffer.
 207           gpsChar = Serial.read() ;
 208 #ifdef DEBUG_FORWARD_GPS_MSG_TO_PC
 209           if (gpsChar == 0xB5 ) {
 210               Serial.println() ; Serial.print(gpsChar , HEX) ;
 211           } else {
 212               Serial.print(F(",")) ; Serial.print(gpsChar , HEX) ;
 213           }
 214 #endif // end DEBUG_FORWARD_GPS_MSG_TO_PC
 215           // process each incoming byte and so fill GPS data when a frame is received and correct
 216           gpsNewFrameUBLOX( gpsChar );
 217       }
 218 #endif
 219 }
 220
 221 bool OXS_GPS::gpsNewFrameUBLOX(uint8_t data) // handle one byte and if this byte is the last one of a valid frame, parse the whole frame to GPS data
 222 {
 223   static uint16_t gpsDataErrors;
 224
 225   static uint8_t _ck_a;// Packet checksum accumulators
 226   static uint8_t _ck_b;// Packet checksum accumulators
 227   static bool _skip_packet = false ;
 228   static uint8_t _step;
 229   static uint8_t _class;
 230   static uint16_t _payload_length;
 231   static uint16_t _payload_counter;
 232
 233
 234     bool parsed = false;
 235 //    printer->print(_step);  printer->print(" "); printer->println(data , HEX);
 236     switch (_step) {
 237         case 0: // Sync char 1 (0xB5)
 238             if ( 0xB5 == data ) { // UBX sync char 1
 239                 _skip_packet = false;
 240                 _step++;
 241             }
 242             break;
 243         case 1: // Sync char 2 (0x62)
 244             if ( 0x62 != data) { // UBX sync char 1
 245                 _step = 0;
 246                 break;
 247             }
 248             _step++;
 249             break;
 250         case 2: // Class
 251             _step++;
 252             _class = data;  // normally we should check that the class is the expected (otherwise, frame should be skipped)
 253             _ck_b = _ck_a = data;   // reset the checksum accumulators
 254             if ( 0x01 != data ) { // we handle only message type = 0x01 = NAVigation message.
 255                 _skip_packet = true; // when skip packet = true, then the wholepacket will be read but discarded.
 256             }
 257             break;
 258         case 3: // Id
 259             _step++;
 260             _ck_b += (_ck_a += data);       // checksum byte
 261             _msg_id = data;
 262             break;
 263         case 4: // Payload length (part 1)
 264             _step++;
 265             _ck_b += (_ck_a += data);       // checksum byte
 266             _payload_length = data; // payload length low byte
 267             break;
 268         case 5: // Payload length (part 2)
 269             _step++;
 270             _ck_b += (_ck_a += data);       // checksum byte
 271             _payload_length += (uint16_t)(data << 8);
 272             if (_payload_length > UBLOX_PAYLOAD_SIZE) {
 273                 _skip_packet = true; // when skip packet = true, then the wholepacket will be read but discarded.
 274             }
 275             _payload_counter = 0;   // prepare to receive payload
 276             if (_payload_length == 0) {
 277                 _step = 7;
 278             }
 279             break;
 280         case 6:
 281             _ck_b += (_ck_a += data);       // checksum byte
 282             if  (_payload_counter < UBLOX_BUFFER_SIZE)  {
 283                 _buffer.bytes[_payload_counter] = data; // save the content of the payload
 284 //                printer->print(data , HEX);
 285               }
 286             _payload_counter++ ;
 287             if (_payload_counter >= _payload_length) {
 288                 _step++;
 289 //                printer->println(" ");
 290             }
 291             break;
 292         case 7:
 293             _step++;
 294             if (_ck_a != data) {
 295                 _skip_packet = true;          // bad checksum
 296                 gpsDataErrors++;
 297             }
 298             break;
 299         case 8:
 300             _step = 0;
 301             if (_ck_b != data) {
 302                 gpsDataErrors++;
 303                 break;              // bad checksum
 304             }
 305
 306             GPS_packetCount++;
 307 //            printer->print("pac : ");  printer->print(GPS_packetCount); printer->print(",err: "); printer->print(gpsDataErrors); printer->print(",skip: "); printer->println(_skip_packet) ;
 308
 309             if (_skip_packet) {
 310                 break;   // do not parse the packet to be skipped
 311             }
 312                          // if we arive here, it means that a valid frame has been received and that the gpsBuffer contains the data to be parsed
 313             if (UBLOX_parse_gps() && (_class == 0x01) ) {
 314                 parsed = true;
 315             }
 316     }  // end of case
 317     return parsed; // return true if position AND speed have been received (not sure if this is usefull)
 318 }
 319
 320 bool OXS_GPS::UBLOX_parse_gps(void) // move the data from buffer to the different fields
 321 {
 322 // do we have new position information?
 323 static bool _new_position = false ;
 324
 325 // do we have new speed information?
 326 static bool _new_speed = false ;
 327 static bool next_fix = false ;
 328 #if defined(GPS_TRANSMIT_TIME)
 329 // do we have new time information?
 330 static bool _new_time = false ;
 331 #endif
 332     switch (_msg_id) {
 333     case MSG_POSLLH:
 334         //i2c_dataset.time                = _buffer.posllh.time;
 335         GPS_lon = _buffer.posllh.longitude;           // in degree with 7 decimals
 336         GPS_lat = _buffer.posllh.latitude;            // in degree with 7 decimals
 337         GPS_altitude = _buffer.posllh.altitude_msl ;  //alt in mm
 338         if (next_fix) {                               // enable state if a position has been received after a positieve STATUS or SOL
 339             GPS_fix = true ;
 340             if ( GPS_home_lat == 0 ) {
 341               GPS_home_lat = _buffer.posllh.latitude ;  // save home position
 342               GPS_home_lon = _buffer.posllh.longitude ;
 343               GPS_scale = cosf(GPS_home_lat * 1.0e-7f * DEG_TO_RAD_FOR_GPS); // calculate scale factor based on latitude
 344             }
 345             // Calculate distance
 346             float dlat  = (float)(GPS_home_lat - GPS_lat);
 347             float dlong  = ((float)(GPS_home_lon - GPS_lon)) * GPS_scale ;
 348             GPS_distance =  sqrtf( dlat * dlat + dlong * dlong  ) * LOCATION_SCALING_FACTOR;
 349             // calculate bearing
 350             int32_t off_x = GPS_lon - GPS_home_lon ;
 351             int32_t off_y = (GPS_lat - GPS_home_lat) / GPS_scale ;
 352             GPS_bearing = 90 + atan2f(-off_y, off_x) * 57.2957795f;  // in degree
 353             if (GPS_bearing < 0) GPS_bearing += 360;
 354 #if defined( A_LOCATOR_IS_CONNECTED)  && ( A_LOCATOR_IS_CONNECTED == YES)
 355             GPS_last_fix_millis = millis() ;  // used by lora locator
 356             GPS_last_fix_lon = GPS_lon ;      // used by lora locator
 357             GPS_last_fix_lat = GPS_lat ;      // used by lora locator
 358 #endif
 359         } else {
 360             GPS_fix = false;
 361         }
 362         GPS_lonAvailable = GPS_latAvailable = GPS_altitudeAvailable = GPS_fix;
 363         _new_position = true;
 364 #ifdef DEBUGPARSEGPS
 365   printer->print(F("Gps fix: "));
 366   printer->print(GPS_fix);
 367   printer->print(F(" long: "));
 368   printer->print(GPS_lon);
 369   printer->print(F(" lat: "));
 370   printer->print(GPS_lat);
 371   printer->print(F(" alt: "));
 372   printer->println(GPS_altitude);
 373 #endif
 374         break;
 375 //    case MSG_STATUS:                              // !!!!!!!!! I do not see real need of this message because same (and more) data are in SOL, so this message is not activated in init
 376 //        next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); // si valid position and speed or 3D fix
 377 //        if (!next_fix)
 378 //             GPS_fix = false;
 379 //        break;
 380     case MSG_SOL:                                // !!!! here we could also use vertical speed which is I4 in cm/sec)
 381         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
 382         GPS_fix_type = _buffer.solution.fix_type;
 383         if (!next_fix)
 384              GPS_fix = false;
 385         GPS_numSat = _buffer.solution.satellites;
 386         GPS_hdop = _buffer.solution.position_DOP;
 387         break;
 388     case MSG_VELNED:   // here we should add the 3d speed (if accurate enough
 389         GPS_speed_3d  = _buffer.velned.speed_3d;  // cm/s
 390         GPS_speed_3dAvailable = GPS_fix ;
 391         GPS_speed_2d = _buffer.velned.speed_2d;    // cm/s
 392         GPS_speed_2dAvailable = GPS_fix ;
 393         GPS_ground_course = _buffer.velned.heading_2d ;     // Heading 2D deg with 5 decimals
 394         GPS_ground_courseAvailable = GPS_fix ;
 395         _new_speed = true;
 396 #ifdef DEBUGPARSEGPS
 397   printer->print(F("Gps spd 3d: "));
 398   printer->print(GPS_speed_3d);
 399   printer->print(F(" 2d: "));
 400   printer->print(GPS_speed_2d);
 401   printer->print(F(" course: "));
 402   printer->println(GPS_ground_course);
 403 #endif
 404         break;
 405 #if defined(GPS_TRANSMIT_TIME)
 406     case MSG_TIMEUTC:   // Parse time informations
 407         if (_buffer.timeutc.flag & 0b111) {
 408           GPS_year=_buffer.timeutc.year;
 409           GPS_month=_buffer.timeutc.month;
 410           GPS_day=_buffer.timeutc.day;
 411           GPS_hour=_buffer.timeutc.hour;
 412           GPS_min=_buffer.timeutc.min;
 413           GPS_sec=_buffer.timeutc.sec;
 414           _new_time=true;
 415           GPS_timeAvailable=true;
 416 #ifdef DEBUGPARSEGPS
 417   printer->print(F("Gps time: "));
 418   printer->print(GPS_year);
 419   printer->print(F("-"));
 420   printer->print(GPS_month);
 421   printer->print(F("-"));
 422   printer->print(GPS_day);
 423   printer->print(F(" "));
 424   printer->print(GPS_hour);
 425   printer->print(F(":"));
 426   printer->print(GPS_min);
 427   printer->print(F(":"));
 428   printer->println(GPS_sec);
 429 #endif
 430         } else {
 431           GPS_timeAvailable=false;
 432         }
 433         break;
 434 #endif
 435     default:
 436         return false;
 437     } // end of case
 438
 439     // we only return true when we get new position and speed data
 440     // this ensures we don't use stale data
 441 #if defined(GPS_TRANSMIT_TIME)
 442     if (_new_position && _new_speed && _new_time) {
 443         _new_speed = _new_position = _new_time = false;
 444         return true;
 445     }
 446 #else
 447     if (_new_position && _new_speed) {
 448         _new_speed = _new_position =  false;
 449         return true;
 450     }
 451 #endif
 452     return false;
 453 }