Jurica Resetar / Mbed OS aconnoGnss

aconno gnss simple example for U-blox gnss dev board.

Fork of gnss by u-blox

main.cpp@8:ca4d0ae32c93, 2018-09-27 (annotated)

Committer:
jurica238814
Date:
Thu Sep 27 10:48:32 2018 +0000
Revision:
8:ca4d0ae32c93
Parent:
TESTS/unit_tests/default/main.cpp@6:56eda66d585b 
aconno Gnss example for u-blox C030.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rob.meades@u-blox.com 1:ef70a58a6c98 1 #include "mbed.h"
rob.meades@u-blox.com 1:ef70a58a6c98 2 #include "greentea-client/test_env.h"
rob.meades@u-blox.com 1:ef70a58a6c98 3 #include "unity.h"
rob.meades@u-blox.com 1:ef70a58a6c98 4 #include "utest.h"
rob.meades@u-blox.com 1:ef70a58a6c98 5 #include "gnss.h"
rob.meades@u-blox.com 1:ef70a58a6c98 6
rob.meades@u-blox.com 1:ef70a58a6c98 7 using namespace utest::v1;
rob.meades@u-blox.com 1:ef70a58a6c98 8
rob.meades@u-blox.com 1:ef70a58a6c98 9 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 10 // COMPILE-TIME MACROS
rob.meades@u-blox.com 1:ef70a58a6c98 11 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 12
rob.meades@u-blox.com 1:ef70a58a6c98 13 // How long to wait for a GNSS result
rob.meades@u-blox.com 1:ef70a58a6c98 14 #define GNSS_WAIT_SECONDS 120
rob.meades@u-blox.com 1:ef70a58a6c98 15
rob.meades@u-blox.com 1:ef70a58a6c98 16 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 17 // PRIVATE VARIABLES
rob.meades@u-blox.com 1:ef70a58a6c98 18 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 19
rob.meades@u-blox.com 1:ef70a58a6c98 20 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 21 // PRIVATE FUNCTIONS
rob.meades@u-blox.com 1:ef70a58a6c98 22 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 23
rob.meades@u-blox.com 1:ef70a58a6c98 24 static void printHex (char * pData, uint32_t lenData)
rob.meades@u-blox.com 1:ef70a58a6c98 25 {
rob.meades@u-blox.com 1:ef70a58a6c98 26 char * pEnd = pData + lenData;
rob.meades@u-blox.com 1:ef70a58a6c98 27 uint8_t x;
rob.meades@u-blox.com 1:ef70a58a6c98 28
rob.meades@u-blox.com 1:ef70a58a6c98 29 printf (" 0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
rob.meades@u-blox.com 1:ef70a58a6c98 30 while (pData < pEnd) {
rob.meades@u-blox.com 1:ef70a58a6c98 31 for (x = 1; (x <= 32) && (pData < pEnd); x++) {
rob.meades@u-blox.com 1:ef70a58a6c98 32 if (x % 16 == 8) {
rob.meades@u-blox.com 1:ef70a58a6c98 33 printf ("%02x ", *pData);
rob.meades@u-blox.com 1:ef70a58a6c98 34 } else if (x % 16 == 0) {
rob.meades@u-blox.com 1:ef70a58a6c98 35 printf ("%02x\n", *pData);
rob.meades@u-blox.com 1:ef70a58a6c98 36 } else {
rob.meades@u-blox.com 1:ef70a58a6c98 37 printf ("%02x-", *pData);
rob.meades@u-blox.com 1:ef70a58a6c98 38 }
rob.meades@u-blox.com 1:ef70a58a6c98 39 pData++;
rob.meades@u-blox.com 1:ef70a58a6c98 40 }
rob.meades@u-blox.com 1:ef70a58a6c98 41
rob.meades@u-blox.com 1:ef70a58a6c98 42
rob.meades@u-blox.com 1:ef70a58a6c98 43 if (x % 16 != 1) {
rob.meades@u-blox.com 1:ef70a58a6c98 44 printf("\n");
rob.meades@u-blox.com 1:ef70a58a6c98 45 }
rob.meades@u-blox.com 1:ef70a58a6c98 46 }
rob.meades@u-blox.com 1:ef70a58a6c98 47 }
rob.meades@u-blox.com 1:ef70a58a6c98 48
rob.meades@u-blox.com 1:ef70a58a6c98 49 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 50 // TESTS
rob.meades@u-blox.com 1:ef70a58a6c98 51 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 52
rob.meades@u-blox.com 1:ef70a58a6c98 53 // Test sending a u-blox command over serial
rob.meades@u-blox.com 1:ef70a58a6c98 54 void test_serial_ubx() {
rob.meades@u-blox.com 1:ef70a58a6c98 55 char buffer[64];
rob.meades@u-blox.com 1:ef70a58a6c98 56 int responseLength = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 57 int returnCode;
rob.meades@u-blox.com 6:56eda66d585b 58 bool gotAck = false;
rob.meades@u-blox.com 6:56eda66d585b 59 Timer timer;
rob.meades@u-blox.com 1:ef70a58a6c98 60
rob.meades@u-blox.com 1:ef70a58a6c98 61 GnssSerial *pGnss = new GnssSerial();
rob.meades@u-blox.com 1:ef70a58a6c98 62
rob.meades@u-blox.com 6:56eda66d585b 63 // Initialise the GNSS chip
rob.meades@u-blox.com 1:ef70a58a6c98 64 pGnss->init(NC);
rob.meades@u-blox.com 1:ef70a58a6c98 65
rob.meades@u-blox.com 6:56eda66d585b 66 // Try this a few times as we might get no response
rob.meades@u-blox.com 6:56eda66d585b 67 // if the GNSS chip is busy
rob.meades@u-blox.com 6:56eda66d585b 68 for (int x = 0; (x < 3) && !gotAck; x++) {
rob.meades@u-blox.com 6:56eda66d585b 69 // See ublox7-V14_ReceiverDescrProtSpec section 30.11.15 (CFG-NAV5)
rob.meades@u-blox.com 6:56eda66d585b 70 // Set automotive mode, which should be acknowledged
rob.meades@u-blox.com 6:56eda66d585b 71 memset (buffer, 0, sizeof (buffer));
rob.meades@u-blox.com 6:56eda66d585b 72 buffer[0] = 0x00;
rob.meades@u-blox.com 6:56eda66d585b 73 buffer[1] = 0x01; // Mask: set dynamic config only
rob.meades@u-blox.com 6:56eda66d585b 74 buffer[2] = 0x04; // Dynamic platform model: automotive
rob.meades@u-blox.com 6:56eda66d585b 75 // Send length is 32 bytes of payload + 6 bytes header + 2 bytes CRC
rob.meades@u-blox.com 6:56eda66d585b 76 TEST_ASSERT_EQUAL_INT (40, pGnss->sendUbx(0x06, 0x24, buffer, 32));
rob.meades@u-blox.com 6:56eda66d585b 77 printf ("CFG_NAV5 command sent, try %d.\n", x);
rob.meades@u-blox.com 6:56eda66d585b 78 timer.start();
rob.meades@u-blox.com 6:56eda66d585b 79 while ((!gotAck) && (timer.read_ms() < 1000)) {
rob.meades@u-blox.com 6:56eda66d585b 80 // Wait for the required Ack
rob.meades@u-blox.com 6:56eda66d585b 81 returnCode = pGnss->getMessage(buffer, sizeof(buffer));
rob.meades@u-blox.com 6:56eda66d585b 82 if ((returnCode != GnssSerial::WAIT) && (returnCode != GnssSerial::NOT_FOUND)) {
rob.meades@u-blox.com 6:56eda66d585b 83 responseLength = LENGTH(returnCode);
rob.meades@u-blox.com 6:56eda66d585b 84 if ((PROTOCOL(returnCode) == GnssSerial::UBX)) {
rob.meades@u-blox.com 6:56eda66d585b 85 printHex(buffer, responseLength);
rob.meades@u-blox.com 6:56eda66d585b 86 // Ack is 0xb5-62-05-00-02-00-msgclass-msgid-crcA-crcB
rob.meades@u-blox.com 6:56eda66d585b 87 // Nack is 0xb5-62-05-01-02-00-msgclass-msgid-crcA-crcB
rob.meades@u-blox.com 6:56eda66d585b 88 TEST_ASSERT_EQUAL_UINT8(0xb5, buffer[0]);
rob.meades@u-blox.com 6:56eda66d585b 89 TEST_ASSERT_EQUAL_UINT8(0x62, buffer[1]);
rob.meades@u-blox.com 6:56eda66d585b 90 TEST_ASSERT_EQUAL_UINT8(0x05, buffer[2]);
rob.meades@u-blox.com 6:56eda66d585b 91 TEST_ASSERT_EQUAL_UINT8(0x00, buffer[3]);
rob.meades@u-blox.com 6:56eda66d585b 92 TEST_ASSERT_EQUAL_UINT8(0x02, buffer[4]);
rob.meades@u-blox.com 6:56eda66d585b 93 TEST_ASSERT_EQUAL_UINT8(0x00, buffer[5]);
rob.meades@u-blox.com 6:56eda66d585b 94 TEST_ASSERT_EQUAL_UINT8(0x06, buffer[6]);
rob.meades@u-blox.com 6:56eda66d585b 95 TEST_ASSERT_EQUAL_UINT8(0x24, buffer[7]);
rob.meades@u-blox.com 6:56eda66d585b 96 gotAck = true;
rob.meades@u-blox.com 6:56eda66d585b 97 } else if ((PROTOCOL(returnCode) == GnssSerial::NMEA)) {
rob.meades@u-blox.com 6:56eda66d585b 98 printf ("%.*s", responseLength, buffer);
rob.meades@u-blox.com 6:56eda66d585b 99 } else {
rob.meades@u-blox.com 6:56eda66d585b 100 printHex(buffer, responseLength);
rob.meades@u-blox.com 6:56eda66d585b 101 }
rob.meades@u-blox.com 1:ef70a58a6c98 102 }
rob.meades@u-blox.com 6:56eda66d585b 103 wait_ms (100);
rob.meades@u-blox.com 1:ef70a58a6c98 104 }
rob.meades@u-blox.com 6:56eda66d585b 105 timer.stop();
rob.meades@u-blox.com 6:56eda66d585b 106 timer.reset();
rob.meades@u-blox.com 1:ef70a58a6c98 107 }
rob.meades@u-blox.com 1:ef70a58a6c98 108 }
rob.meades@u-blox.com 1:ef70a58a6c98 109
rob.meades@u-blox.com 1:ef70a58a6c98 110 // Test getting a response from GNSS using the serial interface
rob.meades@u-blox.com 1:ef70a58a6c98 111 void test_serial_time() {
rob.meades@u-blox.com 1:ef70a58a6c98 112 GnssSerial *pGnss = new GnssSerial();
rob.meades@u-blox.com 1:ef70a58a6c98 113
rob.meades@u-blox.com 1:ef70a58a6c98 114 bool gotLatLong = false;
rob.meades@u-blox.com 1:ef70a58a6c98 115 bool gotElevation = false;
rob.meades@u-blox.com 1:ef70a58a6c98 116 bool gotSpeed = false;
rob.meades@u-blox.com 1:ef70a58a6c98 117 bool gotTime = false;
rob.meades@u-blox.com 1:ef70a58a6c98 118 char buffer[256];
rob.meades@u-blox.com 1:ef70a58a6c98 119 int returnCode;
rob.meades@u-blox.com 1:ef70a58a6c98 120 double latitude;
rob.meades@u-blox.com 1:ef70a58a6c98 121 double longitude;
rob.meades@u-blox.com 1:ef70a58a6c98 122 double elevation;
rob.meades@u-blox.com 1:ef70a58a6c98 123 double speed;
rob.meades@u-blox.com 1:ef70a58a6c98 124
rob.meades@u-blox.com 1:ef70a58a6c98 125 printf("GNSS: powering up and waiting up to %d second(s) for something to happen.\n", GNSS_WAIT_SECONDS);
rob.meades@u-blox.com 1:ef70a58a6c98 126 pGnss->init();
rob.meades@u-blox.com 1:ef70a58a6c98 127
rob.meades@u-blox.com 1:ef70a58a6c98 128 memset(buffer, 0, sizeof(buffer));
rob.meades@u-blox.com 1:ef70a58a6c98 129 for (uint32_t x = 0; (x < GNSS_WAIT_SECONDS) && !gotTime; x++)
rob.meades@u-blox.com 1:ef70a58a6c98 130 {
rob.meades@u-blox.com 1:ef70a58a6c98 131 while (((returnCode = pGnss->getMessage(buffer, sizeof(buffer))) > 0) &&
rob.meades@u-blox.com 1:ef70a58a6c98 132 !(gotLatLong && gotElevation && gotSpeed && gotTime))
rob.meades@u-blox.com 1:ef70a58a6c98 133 {
rob.meades@u-blox.com 1:ef70a58a6c98 134 int32_t length = LENGTH(returnCode);
rob.meades@u-blox.com 1:ef70a58a6c98 135
rob.meades@u-blox.com 1:ef70a58a6c98 136 if ((PROTOCOL(returnCode) == GnssParser::NMEA) && (length > 6))
rob.meades@u-blox.com 1:ef70a58a6c98 137 {
rob.meades@u-blox.com 1:ef70a58a6c98 138 printf(".");
rob.meades@u-blox.com 1:ef70a58a6c98 139
rob.meades@u-blox.com 1:ef70a58a6c98 140 // talker is $GA=Galileo $GB=Beidou $GL=Glonass $GN=Combined $GP=GNSS
rob.meades@u-blox.com 1:ef70a58a6c98 141 if ((buffer[0] == '$') || buffer[1] == 'G')
rob.meades@u-blox.com 1:ef70a58a6c98 142 {
rob.meades@u-blox.com 1:ef70a58a6c98 143 #define _CHECK_TALKER(s) ((buffer[3] == s[0]) && (buffer[4] == s[1]) && (buffer[5] == s[2]))
rob.meades@u-blox.com 1:ef70a58a6c98 144 if (_CHECK_TALKER("GLL"))
rob.meades@u-blox.com 1:ef70a58a6c98 145 {
rob.meades@u-blox.com 1:ef70a58a6c98 146 char ch;
rob.meades@u-blox.com 1:ef70a58a6c98 147
rob.meades@u-blox.com 1:ef70a58a6c98 148 if (pGnss->getNmeaAngle(1, buffer, length, latitude) &&
rob.meades@u-blox.com 1:ef70a58a6c98 149 pGnss->getNmeaAngle(3, buffer, length, longitude) &&
rob.meades@u-blox.com 1:ef70a58a6c98 150 pGnss->getNmeaItem(6, buffer, length, ch) &&
rob.meades@u-blox.com 1:ef70a58a6c98 151 ch == 'A')
rob.meades@u-blox.com 1:ef70a58a6c98 152 {
rob.meades@u-blox.com 1:ef70a58a6c98 153 gotLatLong = true;
rob.meades@u-blox.com 1:ef70a58a6c98 154 latitude *= 60000;
rob.meades@u-blox.com 1:ef70a58a6c98 155 longitude *= 60000;
rob.meades@u-blox.com 1:ef70a58a6c98 156 printf("\nGNSS: location %.5f %.5f %c.\n", latitude, longitude, ch);
rob.meades@u-blox.com 1:ef70a58a6c98 157 }
rob.meades@u-blox.com 1:ef70a58a6c98 158 }
rob.meades@u-blox.com 1:ef70a58a6c98 159 else if (_CHECK_TALKER("GGA") || _CHECK_TALKER("GNS"))
rob.meades@u-blox.com 1:ef70a58a6c98 160 {
rob.meades@u-blox.com 1:ef70a58a6c98 161 const char *pTimeString = NULL;
rob.meades@u-blox.com 1:ef70a58a6c98 162
rob.meades@u-blox.com 1:ef70a58a6c98 163 // Retrieve the time
rob.meades@u-blox.com 1:ef70a58a6c98 164 pTimeString = pGnss->findNmeaItemPos(1, buffer, buffer + length);
rob.meades@u-blox.com 1:ef70a58a6c98 165 if (pTimeString != NULL)
rob.meades@u-blox.com 1:ef70a58a6c98 166 {
rob.meades@u-blox.com 1:ef70a58a6c98 167 gotTime = true;
rob.meades@u-blox.com 1:ef70a58a6c98 168 printf("\nGNSS: time is %.6s.", pTimeString);
rob.meades@u-blox.com 1:ef70a58a6c98 169 }
rob.meades@u-blox.com 1:ef70a58a6c98 170
rob.meades@u-blox.com 1:ef70a58a6c98 171 if (pGnss->getNmeaItem(9, buffer, length, elevation)) // altitude msl [m]
rob.meades@u-blox.com 1:ef70a58a6c98 172 {
rob.meades@u-blox.com 1:ef70a58a6c98 173 gotElevation = true;
rob.meades@u-blox.com 1:ef70a58a6c98 174 printf("\nGNSS: elevation: %.1f.", elevation);
rob.meades@u-blox.com 1:ef70a58a6c98 175 }
rob.meades@u-blox.com 1:ef70a58a6c98 176 }
rob.meades@u-blox.com 1:ef70a58a6c98 177 else if (_CHECK_TALKER("VTG"))
rob.meades@u-blox.com 1:ef70a58a6c98 178 {
rob.meades@u-blox.com 1:ef70a58a6c98 179 if (pGnss->getNmeaItem(7, buffer, length, speed)) // speed [km/h]
rob.meades@u-blox.com 1:ef70a58a6c98 180 {
rob.meades@u-blox.com 1:ef70a58a6c98 181 gotSpeed = true;
rob.meades@u-blox.com 1:ef70a58a6c98 182 printf("\nGNSS: speed: %.1f.", speed);
rob.meades@u-blox.com 1:ef70a58a6c98 183 }
rob.meades@u-blox.com 1:ef70a58a6c98 184 }
rob.meades@u-blox.com 1:ef70a58a6c98 185 }
rob.meades@u-blox.com 1:ef70a58a6c98 186 }
rob.meades@u-blox.com 1:ef70a58a6c98 187 }
rob.meades@u-blox.com 1:ef70a58a6c98 188
rob.meades@u-blox.com 1:ef70a58a6c98 189 wait_ms(1000);
rob.meades@u-blox.com 1:ef70a58a6c98 190 }
rob.meades@u-blox.com 1:ef70a58a6c98 191
rob.meades@u-blox.com 1:ef70a58a6c98 192 printf("\n");
rob.meades@u-blox.com 1:ef70a58a6c98 193
rob.meades@u-blox.com 1:ef70a58a6c98 194 // Depending on antenna positioning we may not be able to get a GNSS fix but we
rob.meades@u-blox.com 1:ef70a58a6c98 195 // should at least be able to receive the time from a satellite
rob.meades@u-blox.com 1:ef70a58a6c98 196 TEST_ASSERT(gotTime);
rob.meades@u-blox.com 1:ef70a58a6c98 197 }
rob.meades@u-blox.com 1:ef70a58a6c98 198
rob.meades@u-blox.com 1:ef70a58a6c98 199 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 200 // TEST ENVIRONMENT
rob.meades@u-blox.com 1:ef70a58a6c98 201 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 202
rob.meades@u-blox.com 1:ef70a58a6c98 203 // Setup the test environment
rob.meades@u-blox.com 1:ef70a58a6c98 204 utest::v1::status_t test_setup(const size_t number_of_cases) {
rob.meades@u-blox.com 1:ef70a58a6c98 205 // Setup Greentea with a timeout
rob.meades@u-blox.com 1:ef70a58a6c98 206 GREENTEA_SETUP(120, "default_auto");
rob.meades@u-blox.com 1:ef70a58a6c98 207 return verbose_test_setup_handler(number_of_cases);
rob.meades@u-blox.com 1:ef70a58a6c98 208 }
rob.meades@u-blox.com 1:ef70a58a6c98 209
rob.meades@u-blox.com 1:ef70a58a6c98 210 // Test cases
rob.meades@u-blox.com 1:ef70a58a6c98 211 Case cases[] = {
rob.meades@u-blox.com 1:ef70a58a6c98 212 Case("Ubx command", test_serial_ubx),
rob.meades@u-blox.com 1:ef70a58a6c98 213 Case("Get time", test_serial_time),
rob.meades@u-blox.com 1:ef70a58a6c98 214 };
rob.meades@u-blox.com 1:ef70a58a6c98 215
rob.meades@u-blox.com 1:ef70a58a6c98 216 Specification specification(test_setup, cases);
rob.meades@u-blox.com 1:ef70a58a6c98 217
rob.meades@u-blox.com 1:ef70a58a6c98 218 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 219 // MAIN
rob.meades@u-blox.com 1:ef70a58a6c98 220 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 221
rob.meades@u-blox.com 1:ef70a58a6c98 222 int main() {
rob.meades@u-blox.com 1:ef70a58a6c98 223
rob.meades@u-blox.com 1:ef70a58a6c98 224 return !Harness::run(specification);
rob.meades@u-blox.com 1:ef70a58a6c98 225 }
rob.meades@u-blox.com 1:ef70a58a6c98 226
rob.meades@u-blox.com 1:ef70a58a6c98 227 // End Of File