Ka Yu Ho
/
GPS_copy
:)
main.cpp@1:cfbdc934e8c6, 2017-03-18 (annotated)
- Committer:
- kayu
- Date:
- Sat Mar 18 20:49:10 2017 +0000
- Revision:
- 1:cfbdc934e8c6
- Parent:
- 0:447ddbf579d9
VTG not working...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kayu | 0:447ddbf579d9 | 1 | #include "mbed.h" |
kayu | 0:447ddbf579d9 | 2 | |
kayu | 0:447ddbf579d9 | 3 | #define MESS_LEN 100 // maximum message length |
kayu | 0:447ddbf579d9 | 4 | #define MAX_STR 20 |
kayu | 0:447ddbf579d9 | 5 | #define STR_LEN 40 |
kayu | 0:447ddbf579d9 | 6 | |
kayu | 0:447ddbf579d9 | 7 | Serial _gps(A0, A1); //tx,rx |
kayu | 0:447ddbf579d9 | 8 | Serial pc(SERIAL_TX,SERIAL_RX); |
kayu | 0:447ddbf579d9 | 9 | DigitalOut myled(LED1); |
kayu | 0:447ddbf579d9 | 10 | |
kayu | 1:cfbdc934e8c6 | 11 | char nmea[MESS_LEN] = {0x00}; |
kayu | 1:cfbdc934e8c6 | 12 | char test[100]; |
kayu | 0:447ddbf579d9 | 13 | char gp[MAX_STR][STR_LEN]; |
kayu | 0:447ddbf579d9 | 14 | |
kayu | 0:447ddbf579d9 | 15 | |
kayu | 0:447ddbf579d9 | 16 | void read_nmea(void); // read nmea message from gps module |
kayu | 0:447ddbf579d9 | 17 | int chop_message(void); // chop the nmea messeage |
kayu | 1:cfbdc934e8c6 | 18 | void present_gga_array(void); // send a part of the available info to the laptop |
kayu | 1:cfbdc934e8c6 | 19 | void present_vtg_array(void); // send a part of the available info to the laptop |
kayu | 1:cfbdc934e8c6 | 20 | void setNmeaMessages( bool , bool , bool , bool , bool , bool ); |
kayu | 0:447ddbf579d9 | 21 | void setUpdateRate(const uint8_t ); |
kayu | 1:cfbdc934e8c6 | 22 | void vtgMessage(bool); |
kayu | 1:cfbdc934e8c6 | 23 | |
kayu | 0:447ddbf579d9 | 24 | |
kayu | 0:447ddbf579d9 | 25 | int main() { |
kayu | 0:447ddbf579d9 | 26 | printf("HERE!\n"); |
kayu | 0:447ddbf579d9 | 27 | _gps.baud(9600); |
kayu | 1:cfbdc934e8c6 | 28 | //pc.baud(4800); |
kayu | 1:cfbdc934e8c6 | 29 | setUpdateRate(1); |
kayu | 1:cfbdc934e8c6 | 30 | setNmeaMessages(true,false,false,false,false,true); |
kayu | 1:cfbdc934e8c6 | 31 | vtgMessage(true); |
kayu | 1:cfbdc934e8c6 | 32 | //setNavigationMode(0); |
kayu | 1:cfbdc934e8c6 | 33 | while(1){ |
kayu | 0:447ddbf579d9 | 34 | read_nmea(); |
kayu | 1:cfbdc934e8c6 | 35 | |
kayu | 0:447ddbf579d9 | 36 | chop_message(); |
kayu | 1:cfbdc934e8c6 | 37 | present_gga_array(); |
kayu | 1:cfbdc934e8c6 | 38 | // present_vtg_array(); |
kayu | 1:cfbdc934e8c6 | 39 | |
kayu | 1:cfbdc934e8c6 | 40 | |
kayu | 0:447ddbf579d9 | 41 | } |
kayu | 0:447ddbf579d9 | 42 | } |
kayu | 0:447ddbf579d9 | 43 | |
kayu | 0:447ddbf579d9 | 44 | //--------------------read nmea from GPS unit-------------------- |
kayu | 0:447ddbf579d9 | 45 | void read_nmea(void) { |
kayu | 0:447ddbf579d9 | 46 | _gps.scanf("%s,",nmea); |
kayu | 1:cfbdc934e8c6 | 47 | |
kayu | 0:447ddbf579d9 | 48 | pc.printf("Raw message %s \n\r",nmea); // just for testing |
kayu | 0:447ddbf579d9 | 49 | } |
kayu | 0:447ddbf579d9 | 50 | |
kayu | 0:447ddbf579d9 | 51 | //---------------- chop the nmea message separated by comma's----------- |
kayu | 0:447ddbf579d9 | 52 | int chop_message(void) { |
kayu | 0:447ddbf579d9 | 53 | for (int k=0; k<MAX_STR; k++) { // just to make sure thet the char array is set to 0x00 |
kayu | 0:447ddbf579d9 | 54 | for (int l=0; l<STR_LEN; l++) { |
kayu | 0:447ddbf579d9 | 55 | gp[k][l]= 0x00; |
kayu | 0:447ddbf579d9 | 56 | }; |
kayu | 0:447ddbf579d9 | 57 | } |
kayu | 0:447ddbf579d9 | 58 | int strcnt=0; // string counter |
kayu | 0:447ddbf579d9 | 59 | int strpos=0; // position inside the string |
kayu | 0:447ddbf579d9 | 60 | for (int k=0; k < MESS_LEN; k++) { |
kayu | 0:447ddbf579d9 | 61 | if (nmea[k] == '*') { // detect end of message is found |
kayu | 0:447ddbf579d9 | 62 | gp[strcnt][strpos]= 0x00; // close the string with 0x00 |
kayu | 0:447ddbf579d9 | 63 | return 0; // the work is done, end of this function |
kayu | 0:447ddbf579d9 | 64 | } |
kayu | 0:447ddbf579d9 | 65 | if (nmea[k] == 0x2c) { // detect the comma |
kayu | 0:447ddbf579d9 | 66 | if (strpos == 0) { |
kayu | 0:447ddbf579d9 | 67 | gp[strcnt][0]= 'E'; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 68 | gp[strcnt][1]= 'm'; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 69 | gp[strcnt][2]= 'p'; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 70 | gp[strcnt][3]= 't'; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 71 | gp[strcnt][4]= 'y'; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 72 | gp[strcnt][5]= 0x00; // comma at position zero, string must be empty |
kayu | 0:447ddbf579d9 | 73 | } else { |
kayu | 0:447ddbf579d9 | 74 | gp[strcnt][strpos]= 0x00; // end the previous string |
kayu | 0:447ddbf579d9 | 75 | } |
kayu | 0:447ddbf579d9 | 76 | strcnt += 1; // increment to the next string |
kayu | 0:447ddbf579d9 | 77 | strpos =0; // start at position zero |
kayu | 0:447ddbf579d9 | 78 | } else { |
kayu | 0:447ddbf579d9 | 79 | gp[strcnt][strpos]= nmea[k]; // add char to string |
kayu | 0:447ddbf579d9 | 80 | strpos += 1; |
kayu | 0:447ddbf579d9 | 81 | } |
kayu | 0:447ddbf579d9 | 82 | } |
kayu | 0:447ddbf579d9 | 83 | return 0; |
kayu | 0:447ddbf579d9 | 84 | } |
kayu | 0:447ddbf579d9 | 85 | |
kayu | 0:447ddbf579d9 | 86 | // --------------- Dump only $GPGGA on the screen------------------ |
kayu | 1:cfbdc934e8c6 | 87 | void present_gga_array(void) { |
kayu | 0:447ddbf579d9 | 88 | if ((gp[0][2]=='P') and (gp[0][3]=='G') and (gp[0][4]=='G') and (gp[0][5]=='A')) { |
kayu | 0:447ddbf579d9 | 89 | pc.printf("Chop results "); |
kayu | 0:447ddbf579d9 | 90 | for (int k=0; k<MAX_STR; k++) { |
kayu | 0:447ddbf579d9 | 91 | pc.printf("r%d=%s ",k, gp[k]); |
kayu | 0:447ddbf579d9 | 92 | } |
kayu | 0:447ddbf579d9 | 93 | pc.printf("\n\r"); |
kayu | 0:447ddbf579d9 | 94 | } |
kayu | 0:447ddbf579d9 | 95 | } |
kayu | 1:cfbdc934e8c6 | 96 | |
kayu | 1:cfbdc934e8c6 | 97 | // --------------- Dump only $GPVTG on the screen------------------ |
kayu | 1:cfbdc934e8c6 | 98 | void present_vtg_array(void) { |
kayu | 1:cfbdc934e8c6 | 99 | if ( (gp[0][3]=='V') and (gp[0][4]=='T') and (gp[0][5]=='G')) { |
kayu | 1:cfbdc934e8c6 | 100 | pc.printf("Chop results "); |
kayu | 1:cfbdc934e8c6 | 101 | for (int k=0; k<MAX_STR; k++) { |
kayu | 1:cfbdc934e8c6 | 102 | pc.printf("r%d=%s ",k, gp[k]); |
kayu | 1:cfbdc934e8c6 | 103 | } |
kayu | 1:cfbdc934e8c6 | 104 | pc.printf("\n\r"); |
kayu | 1:cfbdc934e8c6 | 105 | } |
kayu | 1:cfbdc934e8c6 | 106 | } |
kayu | 0:447ddbf579d9 | 107 | //----------------Set Msg Command----------------------------- |
kayu | 1:cfbdc934e8c6 | 108 | void setNmeaMessages( bool gga, bool gsa, bool gsv, bool gll, bool rmc, bool vtg) { |
kayu | 1:cfbdc934e8c6 | 109 | |
kayu | 1:cfbdc934e8c6 | 110 | //A0, A1, 00, 09 |
kayu | 1:cfbdc934e8c6 | 111 | //08, 01, 01, 01, 00, 01, 00, 00, 00, |
kayu | 1:cfbdc934e8c6 | 112 | //08 0D 0A |
kayu | 1:cfbdc934e8c6 | 113 | |
kayu | 1:cfbdc934e8c6 | 114 | char cmd[15] = { |
kayu | 0:447ddbf579d9 | 115 | 0xA0, 0xA1, 0x00, 0x09, |
kayu | 1:cfbdc934e8c6 | 116 | 0x08, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01,0, |
kayu | 0:447ddbf579d9 | 117 | 0x00, 0x0D, 0x0A |
kayu | 0:447ddbf579d9 | 118 | }; |
kayu | 1:cfbdc934e8c6 | 119 | |
kayu | 1:cfbdc934e8c6 | 120 | for(int i = 4; i < 12; i++) cmd[12] ^= cmd[i]; |
kayu | 1:cfbdc934e8c6 | 121 | for(int i = 0; i < 15; i++) _gps.putc(cmd[i]); |
kayu | 0:447ddbf579d9 | 122 | } |
kayu | 0:447ddbf579d9 | 123 | |
kayu | 0:447ddbf579d9 | 124 | //----------------Set Update Rate----------------------------- |
kayu | 0:447ddbf579d9 | 125 | void setUpdateRate(const uint8_t rate) { |
kayu | 0:447ddbf579d9 | 126 | char cmd[10] = { |
kayu | 0:447ddbf579d9 | 127 | 0xA0, 0xA1, 0x00, 0x03, |
kayu | 0:447ddbf579d9 | 128 | 0x0E, rate, 0x01, |
kayu | 0:447ddbf579d9 | 129 | 0x00, 0x0D, 0x0A |
kayu | 0:447ddbf579d9 | 130 | }; |
kayu | 0:447ddbf579d9 | 131 | for(int i = 4; i < 7; i++) cmd[7] ^= cmd[i]; |
kayu | 0:447ddbf579d9 | 132 | for(int i = 0; i < 10; i++) _gps.putc(cmd[i]); |
kayu | 1:cfbdc934e8c6 | 133 | } |
kayu | 1:cfbdc934e8c6 | 134 | |
kayu | 1:cfbdc934e8c6 | 135 | //----------------Enable VTG--------------------------------- |
kayu | 1:cfbdc934e8c6 | 136 | void vtgMessage(bool enable){ |
kayu | 1:cfbdc934e8c6 | 137 | if (enable) { |
kayu | 1:cfbdc934e8c6 | 138 | _gps.printf("$PSRF103,05,00,01,01*20\r\n"); // Enable VTG |
kayu | 1:cfbdc934e8c6 | 139 | } |
kayu | 1:cfbdc934e8c6 | 140 | else { |
kayu | 1:cfbdc934e8c6 | 141 | _gps.printf("$PSRF103,05,00,00,01*21\r\n"); // Disable VTG |
kayu | 1:cfbdc934e8c6 | 142 | } |
kayu | 1:cfbdc934e8c6 | 143 | } |
kayu | 1:cfbdc934e8c6 | 144 | |
kayu | 1:cfbdc934e8c6 | 145 | //---------------Set Navigation Mode-------------------------- |
kayu | 1:cfbdc934e8c6 | 146 | void setNavigationMode(const int mode) { |
kayu | 1:cfbdc934e8c6 | 147 | char cmd[] = { |
kayu | 1:cfbdc934e8c6 | 148 | 0xA0, 0xA1, 0x00, 0x02, |
kayu | 1:cfbdc934e8c6 | 149 | 0xB5, 0x00, |
kayu | 1:cfbdc934e8c6 | 150 | 0x00, 0x0D, 0x0A |
kayu | 1:cfbdc934e8c6 | 151 | }; |
kayu | 1:cfbdc934e8c6 | 152 | for(int i = 4; i < 6; i++) cmd[6] ^= cmd[i]; |
kayu | 1:cfbdc934e8c6 | 153 | for(int i = 0; i < 10; i++) _gps.putc(cmd[i]); |
kayu | 1:cfbdc934e8c6 | 154 | } |
kayu | 1:cfbdc934e8c6 | 155 |