Diff: main.cpp

Revision:

52:4c282feb57eb

Parent:

51:6a158dcc7457

Child:

53:50ecf1240eba

diff -r 6a158dcc7457 -r 4c282feb57eb main.cpp
--- a/main.cpp	Thu Jun 02 13:56:39 2022 +0200
+++ b/main.cpp	Fri Jun 03 11:54:52 2022 +0200
@@ -1,218 +1,3 @@
-#include <mbed.h>
-//#include "greentea-client/test_env.h"
-//#include "unity.h"
-//#include "utest.h"
-#include <gnss.h>
- 
-//using namespace utest::v1;
-
-// ----------------------------------------------------------------
-// COMPILE-TIME MACROS
-// ----------------------------------------------------------------
-
-// How long to wait for a GNSS result
-#define GNSS_WAIT_SECONDS 120
-
-// ----------------------------------------------------------------
-// PRIVATE VARIABLES
-// ----------------------------------------------------------------
-
-// ----------------------------------------------------------------
-// PRIVATE FUNCTIONS
-// ----------------------------------------------------------------
-
-static void printHex (char * pData, uint32_t lenData)
-{
-    char * pEnd = pData + lenData;
-    uint8_t x;
-
-    printf (" 0  1  2  3  4  5  6  7   8  9  A  B  C  D  E  F\n");
-    while (pData < pEnd) {
-        for (x = 1; (x <= 32) && (pData < pEnd); x++) {
-            if (x % 16 == 8) {
-                printf ("%02x  ", *pData);
-            } else if (x % 16 == 0) {
-                printf ("%02x\n", *pData);
-            } else {
-                printf ("%02x-", *pData);
-            }
-            pData++;
-        }
-
-
-        if (x % 16 !=  1) {
-            printf("\n");
-        }
-    }
-}
-
-// ----------------------------------------------------------------
-// TESTS
-// ----------------------------------------------------------------
-
-// Test sending a u-blox command over serial
-void test_serial_ubx() {
-    char buffer[64];
-    int responseLength = 0;
-    int returnCode;
-    bool gotAck = false;
-    Timer timer;
-
-    GnssSerial *pGnss = new GnssSerial(PA_9, PA_10, 38400);
-
-    // Initialise the GNSS chip
-    pGnss->init(NC);
-
-    // Try this a few times as we might get no response
-    // if the GNSS chip is busy
-    for (int x = 0; (x < 3) && !gotAck; x++) {
-        // See ublox7-V14_ReceiverDescrProtSpec section 30.11.15 (CFG-NAV5)
-        // Set automotive mode, which should be acknowledged
-        memset (buffer, 0, sizeof (buffer));
-        buffer[0] = 0x00;
-        buffer[1] = 0x01; // Mask: set dynamic config only
-        buffer[2] = 0x04; // Dynamic platform model: automotive
-        // Send length is 32 bytes of payload + 6 bytes header + 2 bytes CRC
-        //TEST_ASSERT_EQUAL_INT (40, pGnss->sendUbx(0x06, 0x24, buffer, 32));
-        printf ("CFG_NAV5 command sent, try %d.\n", x);
-        timer.start();
-        while ((!gotAck) && (std::chrono::duration_cast<std::chrono::milliseconds>(timer.elapsed_time()).count() < 1000)) {
-            // Wait for the required Ack
-            returnCode = pGnss->getMessage(buffer, sizeof(buffer));
-            if ((returnCode != GnssSerial::WAIT) && (returnCode != GnssSerial::NOT_FOUND)) {
-                responseLength = LENGTH(returnCode);
-                if ((PROTOCOL(returnCode) == GnssSerial::UBX)) {
-                    printHex(buffer, responseLength);
-                    // Ack is  0xb5-62-05-00-02-00-msgclass-msgid-crcA-crcB
-                    // Nack is 0xb5-62-05-01-02-00-msgclass-msgid-crcA-crcB
-                    //TEST_ASSERT_EQUAL_UINT8(0xb5, buffer[0]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x62, buffer[1]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x05, buffer[2]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x00, buffer[3]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x02, buffer[4]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x00, buffer[5]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x06, buffer[6]);
-                    //TEST_ASSERT_EQUAL_UINT8(0x24, buffer[7]);
-                    gotAck = true;
-                } else if ((PROTOCOL(returnCode) == GnssSerial::NMEA)) {
-                    printf ("%.*s", responseLength, buffer);
-                } else {
-                    printHex(buffer, responseLength);
-                }
-            }
-            thread_sleep_for(100); //wait_ms (100);
-        }
-        timer.stop();
-        timer.reset();
-    }
-}
-
-// Test getting a response from GNSS using the serial interface
-void test_serial_time() {
-    GnssSerial *pGnss = new GnssSerial(PA_9, PA_10, 38400);
-
-    bool gotLatLong = false;
-    bool gotElevation = false;
-    bool gotSpeed = false;
-    bool gotTime = false;
-    char buffer[256];
-    int returnCode;
-    double latitude;
-    double longitude;
-    double elevation;
-    double speed;
-
-    printf("GNSS: powering up and waiting up to %d second(s) for something to happen.\n", GNSS_WAIT_SECONDS);
-    pGnss->init();
-
-    memset(buffer, 0, sizeof(buffer));
-    for (uint32_t x = 0; (x < GNSS_WAIT_SECONDS) && !gotTime; x++)
-    {
-        while (((returnCode = pGnss->getMessage(buffer, sizeof(buffer))) > 0) &&
-                !(gotLatLong && gotElevation && gotSpeed && gotTime))
-        {
-            int32_t length = LENGTH(returnCode);
-
-            if ((PROTOCOL(returnCode) == GnssParser::NMEA) && (length > 6))
-            {
-                printf(".");
-
-                // talker is $GA=Galileo $GB=Beidou $GL=Glonass $GN=Combined $GP=GNSS
-                if ((buffer[0] == '$') || buffer[1] == 'G')
-                {
-#define _CHECK_TALKER(s) ((buffer[3] == s[0]) && (buffer[4] == s[1]) && (buffer[5] == s[2]))
-                    if (_CHECK_TALKER("GLL"))
-                    {
-                        char ch;
-
-                        if (pGnss->getNmeaAngle(1, buffer, length, latitude) &&
-                            pGnss->getNmeaAngle(3, buffer, length, longitude) &&
-                            pGnss->getNmeaItem(6, buffer, length, ch) &&
-                            ch == 'A')
-                        {
-                            gotLatLong = true;
-                            latitude *= 60000;
-                            longitude *= 60000;
-                            printf("\nGNSS: location %.5f %.5f %c.\n", latitude, longitude, ch);
-                        }
-                    }
-                    else if (_CHECK_TALKER("GGA") || _CHECK_TALKER("GNS"))
-                    {
-                        const char *pTimeString = NULL;
-
-                        // Retrieve the time
-                        pTimeString = pGnss->findNmeaItemPos(1, buffer, buffer + length);
-                        if (pTimeString != NULL)
-                        {
-                            gotTime = true;
-                            printf("\nGNSS: time is %.6s.", pTimeString);
-                        }
-
-                        if (pGnss->getNmeaItem(9, buffer, length, elevation)) // altitude msl [m]
-                        {
-                            gotElevation = true;
-                            printf("\nGNSS: elevation: %.1f.", elevation);
-                        }
-                    }
-                    else if (_CHECK_TALKER("VTG"))
-                    {
-                        if (pGnss->getNmeaItem(7, buffer, length, speed)) // speed [km/h]
-                        {
-                            gotSpeed = true;
-                            printf("\nGNSS: speed: %.1f.", speed);
-                        }
-                    }
-                }
-            }
-        }
-
-        thread_sleep_for(1000); //wait_ms(1000);
-    }
-
-    printf("\n");
-
-    // Depending on antenna positioning we may not be able to get a GNSS fix but we
-    // should at least be able to receive the time from a satellite
-    //TEST_ASSERT(gotTime);
-}
-
-int main() {
-
-    DigitalOut user_led(LED1);      // create DigitalOut object to command user led
-
-   test_serial_time();
-
-   while (true) {
-       printf("   main ended\r\n");
-       user_led = !user_led;
-       thread_sleep_for(1000);
-   }
-}
-
-// End Of File
-
-
-/*
 #include <mbed.h>
 
 // GNSS and Compass test programm for Mateksys GNSS&Compass M9N-5883
@@ -234,7 +19,7 @@
 int main()
 {
     // while loop gets executed every main_task_period_ms milliseconds
-    const int main_task_period_ms = 100;  // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+    const int main_task_period_ms = 40;  // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
     Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
 
     // led on nucleo board
@@ -252,6 +37,7 @@
     // create object for GNSS Sensor NEO-M9N
     NEOM9N neom9n(PA_9, PA_10); // UART1
     //NEOM9N neom9n(PA_2, PA_3); // UART2
+    //Timer neom9n_timer;
 
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
@@ -260,31 +46,34 @@
     // start timer
     main_task_timer.start();
 
+    //neom9n_timer.start();
+
     while (true) { // this loop will run forever
 
         main_task_timer.reset();
 
-        mag.readMag();
+        mag.readMag(); // this needs approx 2450 mus
+        mag_val[0] = raw_mx2mx.evaluate(mag.magX());
+        mag_val[1] = raw_my2my.evaluate(mag.magY());
+        mag_val[2] = raw_mz2mz.evaluate(mag.magZ());
+
+        //neom9n_timer.reset();
+        neom9n.update();
+        //int neom9n_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(neom9n_timer.elapsed_time()).count();
 
         if (do_execute_main_task) {
 
-            mag_val[0] = raw_mx2mx.evaluate(mag.magX());
-            mag_val[1] = raw_my2my.evaluate(mag.magY());
-            mag_val[2] = raw_mz2mz.evaluate(mag.magZ());
-
         } else {
 
-            for (uint8_t i = 0; i <= 3; i++) {
-                mag_val[i] = 0;
-            }
-
         }
 
         user_led = !user_led;
 
+        int main_task_elapsed_time_mus = std::chrono::duration_cast<std::chrono::microseconds>(main_task_timer.elapsed_time()).count();
+
         // do only output via serial what's really necessary (this makes your code slow)
-        printf("%f, %f, %f\r\n", mag_val[0], mag_val[1], mag_val[2]);
-        printf("GPS time: %d, num sat: %d, lat: %d, lon: %d, speed: %d, heading: %d\r\n", neom9n.actualPVT.itow, neom9n.actualPVT.numSV, neom9n.actualPVT.lat, neom9n.actualPVT.lon, neom9n.actualPVT.speed, neom9n.actualPVT.headMot);
+        // printf("%f, %f, %f\r\n", mag_val[0], mag_val[1], mag_val[2]);
+        printf("Update time: %d, GPS time: %d, num sat: %d, lat: %d, lon: %d, speed: %d, heading: %d\r\n", main_task_elapsed_time_mus, neom9n.actualPVT.itow, neom9n.actualPVT.numSV, neom9n.actualPVT.lat, neom9n.actualPVT.lon, neom9n.actualPVT.speed, neom9n.actualPVT.headMot);
 
         // read timer and make the main thread sleep for the remaining time span (non blocking)
         int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
@@ -306,5 +95,4 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
-}
-*/
\ No newline at end of file
+}
\ No newline at end of file