SCIboard_ubloxGps.cpp

00001 /* SCIboard(TM) SCIboard_ubloxGps.cpp
00002 Copyright (c) 2013 K. Andres
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 #include "SCIboard_ubloxGps.h"
00024 
00025 #define SYNC_CHAR_1 0xB5
00026 #define SYNC_CHAR_2 0x62
00027 
00028 #define MAX_UBLOX_PACKET_SIZE  512
00029 
00030 // UBX Class IDs
00031 #define NAV_CLASS_ID    0x01    // Navigation
00032 #define RXM_CLASS_ID    0x02    // Receiver
00033 #define INF_CLASS_ID    0x04    // Information
00034 #define ACK_CLASS_ID    0x05    // Ack/Nack
00035 #define CFG_CLASS_ID    0x06    // Configuration input
00036 #define MON_CLASS_ID    0x0A    // Monitoring messages
00037 #define AID_CLASS_ID    0x0B    // AssistNow Aiding
00038 #define TIM_CLASS_ID    0x0D    // Timing
00039 #define ESF_CLASS_ID    0x10    // External Sensor Fusion
00040 
00041 
00042 // Default - messages to keep
00043 #define NAV_SOL         0x06    // TOWms, week, ECEF
00044 
00045 // Turn ON some messages
00046 #define NAV_SVINFO      0x30
00047 
00048 // Turn off some default messages
00049 #define NAV_POSLLH      0x02
00050 #define NAV_STATUS      0x03
00051 #define NAV_VELNED      0x12
00052 
00053 #define TIM_TP          0x01
00054 #define TIM_VRFY        0x06
00055 
00056 
00057 extern Serial pc;
00058 extern Serial gps;
00059 
00060 void ublox_gps_callback();
00061 
00062 #define UBLOX_BUF_SIZE  2048
00063 
00064 // Input buffer
00065 char ublox_msg_in[UBLOX_BUF_SIZE];
00066 int nextin, nextout;
00067 
00068 // Output buffer
00069 char ublox_msg_out[UBLOX_BUF_SIZE];
00070 char ublox_cnt;
00071 char ublox_msg_out_flag;
00072 
00073 char packetState;
00074 int ublox_size;
00075 
00076 #define UBLOX_MSG_CLASS     2       // field within msg
00077 #define UBLOX_MSG_ID        3
00078 
00079 void ubloxMsgRate(char msgclass, char id, char rate);
00080 void ubloxBaudRate(void);
00081 
00082 SCIboard_ubloxGps::SCIboard_ubloxGps() {
00083     ublox_cnt = 0;
00084     ublox_msg_out_flag = 0;
00085     nextin=nextout=0;
00086     packetState=0;
00087 }
00088 
00089 FILE *ublox_fp=NULL;
00090 
00091 void SCIboard_ubloxGps::gps_startup(void) {
00092     gps.baud(38400);
00093     wait(.01);
00094     
00095     if(ublox_fp==NULL) {
00096         ublox_fp = fopen("/local/GPS.txt", "w");  // Open "GPS.txt" on the local file system for appending
00097     }
00098 
00099     // Turn off some of the default messages
00100     ubloxMsgRate(TIM_CLASS_ID, TIM_TP, 0);
00101     ubloxMsgRate(TIM_CLASS_ID, TIM_VRFY, 0);
00102     ubloxMsgRate(NAV_CLASS_ID, NAV_VELNED, 0);
00103     ubloxMsgRate(NAV_CLASS_ID, NAV_STATUS, 0);
00104     ubloxMsgRate(NAV_CLASS_ID, NAV_POSLLH, 0);
00105 
00106     // Turn on some messages
00107     ubloxMsgRate(NAV_CLASS_ID, NAV_SOL, 4);         // Change rate to once per second
00108     ubloxMsgRate(NAV_CLASS_ID, NAV_SVINFO, 8);      // Change rate to once per 2 seconds
00109     
00110     void ubloxBaudRate();    // Drop baud rate
00111     
00112     gps.attach(&ublox_gps_callback);
00113 }
00114 
00115 void SCIboard_ubloxGps::gps_close(void) {
00116     gps.attach(NULL);
00117     wait(0.01);
00118     
00119     if(nextin) {
00120         if(ublox_fp!=NULL) {
00121             memcpy(ublox_msg_out, ublox_msg_in, nextin);
00122             fwrite(ublox_msg_out, nextin, 1, ublox_fp);
00123         }  
00124         nextin=0;
00125     }
00126 
00127     if(ublox_fp!=NULL) {
00128         fclose(ublox_fp);
00129         ublox_fp=NULL;
00130     }
00131 }
00132 
00133 
00134 void ublox_gps_callback() {
00135     // Note: you need to actually read from the serial to clear the RX interrupt
00136     __disable_irq();
00137     ublox_msg_in[nextin++] = gps.getc();
00138     __enable_irq();
00139 
00140     if(nextin>=sizeof(ublox_msg_in)) {
00141         nextin=0;
00142         if(ublox_fp!=NULL) {
00143             memcpy(ublox_msg_out, ublox_msg_in, sizeof(ublox_msg_in));
00144             fwrite(ublox_msg_out, sizeof(ublox_msg_out), 1, ublox_fp);
00145         }  
00146     }
00147 }
00148 
00149 
00150 // Change message rate or turn off message
00151 void ubloxMsgRate(char msgclass, char id, char rate) {
00152     unsigned char chkA=0, chkB=0;
00153     int n;
00154     uint8_t buf[] = {
00155         6,               // CLASS: CFG
00156         1,               // ID: MSG
00157         8,               // LENGTH
00158         0,               // LENGTH
00159         msgclass,
00160         id,
00161         0,
00162         rate,
00163         0, 0, 0, 0};
00164         
00165     gps.putc(SYNC_CHAR_1);
00166     gps.putc(SYNC_CHAR_2);
00167     
00168     for(n=0; n<sizeof(buf); n++) {
00169         gps.putc(buf[n]);
00170         chkA += buf[n];
00171         chkB += chkA;    
00172     }
00173     
00174     gps.putc(chkA);
00175     gps.putc(chkB);
00176 }
00177 
00178 
00179 // Change message baud rate --------------------------------
00180 // Baud rate divisors
00181 // 0, 0, 0x96, 0            // 38,400
00182 // 0, 0, 0x4B, 0            // 19,200
00183 // 0, 0x80, 0x25, 0         //  9,600
00184 // 0, 0xC0, 0x12, 0         //  4,800
00185 
00186 void ubloxBaudRate(void) {
00187     unsigned char chkA=0, chkB=0;
00188     int n;
00189     uint8_t buf[] = {
00190         6, 0,            // CLASS/id: CFG-PRT
00191         0x14, 0,         // LENGTH     
00192         01, 0,
00193         0, 0, 0xD0, 8, 0, 0, 0xC0, 0x12,
00194         0, 0, 7, 0, 1, 0, 0, 0,
00195         0, 0};
00196 
00197     gps.putc(SYNC_CHAR_1);
00198     gps.putc(SYNC_CHAR_2);
00199     
00200     for(n=0; n<sizeof(buf); n++) {
00201         gps.putc(buf[n]);
00202         chkA += buf[n];
00203         chkB += chkA;    
00204     }
00205     
00206     gps.putc(chkA);
00207     gps.putc(chkB);
00208 
00209     wait(.500);
00210     gps.baud(4800);
00211 }