main.cpp

00001 #include <mbed.h>
00002 
00003 // GNSS and Compass test programm for Mateksys GNSS&Compass M9N-5883
00004 
00005 #include "Eigen/Dense.h"
00006 #include "QMC5883L.h"
00007 #include "LinearCharacteristics.h"
00008 #include "NEOM9N_thread.h"
00009 
00010 bool do_use_calibrated_mag = true;
00011 
00012 // logical variable main task
00013 bool do_execute_main_task = false;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
00014 
00015 // user button on nucleo board
00016 Timer user_button_timer;            // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
00017 InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
00018 void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
00019 void user_button_released_fcn();
00020 
00021 int main()
00022 {
00023     // while loop gets executed every main_task_period_ms milliseconds
00024     const int main_task_period_ms = 50; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
00025     Timer main_task_timer;              // create Timer object which we use to run the main task every main task period time in ms
00026     main_task_timer.start();
00027     
00028     Timer run_timer;
00029     run_timer.start();
00030 
00031     // led on nucleo board
00032     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
00033 
00034     // create QMC5883L compass object
00035     I2C i2c(PB_4, PA_8);
00036     QMC5883L mag(i2c);
00037     LinearCharacteristics raw_mx2mx, raw_my2my, raw_mz2mz;
00038     if (do_use_calibrated_mag) {
00039         raw_mx2mx.setup(0.9976f, 0.0091f);
00040         raw_my2my.setup(0.9962f, 0.2145f);
00041         raw_mz2mz.setup(1.0062f, -0.0920f);
00042     } else {
00043         raw_mx2mx.setup(1.0f, 0.0f);
00044         raw_my2my.setup(1.0f, 0.0f);
00045         raw_mz2mz.setup(1.0f, 0.0f);
00046     }
00047     float mag_val[3] = {0.0f, 0.0f, 0.0f};
00048     
00049     // create object for GNSS Sensor NEO-M9N, this runs as an own thread
00050     NEOM9N neom9n_thread(PC_6, PC_7);
00051     neom9n_thread.start_loop();
00052     NEOM9N::ubxNavPVT_t ubxNavPVT; // this is only to access raw data
00053     bool isGNSSInit = false;
00054     
00055     // attach button fall and rise functions to user button object
00056     user_button.fall(&user_button_pressed_fcn);
00057     user_button.rise(&user_button_released_fcn);   
00058 
00059     /*
00060     // dev checksum
00061     char msg[100] = {0xB5, 0x62, 0x01, 0x07, 0x5C, 0x00, 0xC8, 0x72, 0xAE, 0x16, 0xE6, 0x07, 0x06, 0x09, 0x09, 0x29,
00062                      0x35, 0xF3, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x84, 0xD7, 0x17, 0x00, 0x00, 0xA4, 0x00, 0x00, 0x00,
00063                      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x98, 0xBD, 0xFF, 0xFF, 0xFF, 0xFF,
00064                      0xFF, 0xFF, 0x00, 0x34, 0xF8, 0xDF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00065                      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x58, 0x3E, 0x0F, 0x00, 0x80, 0xA8,
00066                      0x12, 0x01, 0x0F, 0x27, 0x00, 0x00, 0xEE, 0x13, 0x4F, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00067                      0x00, 0x00, 0xB9, 0x53};
00068     uint8_t CK_A = 0x08; // 0x01 + 0x07
00069     uint8_t CK_B = 0x09; // 0x01 + 0x01 + 0x07;
00070     printf("%02x %02x\r\n", CK_A, CK_B);
00071     printf(" ---\r\n");
00072     bool do_it_once = true;
00073     while(do_it_once) {
00074         for (int i = 0; i<100; i++) {
00075             printf("%02x ", msg[i]);
00076             if ((i + 1)%16 == 0)printf("\r\n");
00077             if (i >= 4 && i<=97) {
00078                 CK_A += msg[i];
00079                 CK_B += CK_A;
00080             }
00081         }
00082         printf("\r\n");
00083         printf(" ---\r\n");
00084         printf("%02x %02x\r\n", CK_A, CK_B);
00085         printf(" ---\r\n");
00086         do_it_once = false;
00087     }
00088     */
00089 
00090     while (true) { // this loop will run forever
00091 
00092         main_task_timer.reset();
00093         //run_timer.reset(); // comment this to get a time reference
00094 
00095         // update magnetometer
00096         mag.readMag(); // this needs approx 2450 mus
00097         mag_val[0] = raw_mx2mx.evaluate( mag.magX() );
00098         mag_val[1] = raw_my2my.evaluate( mag.magY() );
00099         mag_val[2] = raw_mz2mz.evaluate( mag.magZ() );
00100 
00101         // manual zero GNSS
00102         if (do_execute_main_task) {
00103             neom9n_thread.reset_local();
00104             do_execute_main_task = false;
00105         }
00106 
00107         int run_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(run_timer.elapsed_time()).count();
00108 
00109         // readout data and sens via serial to putty
00110         ubxNavPVT = neom9n_thread.GetUbxNavPVT();
00111         printf("%0.3f, %0.3f, %0.3f, ", //  1:3
00112             mag_val[0], mag_val[1], mag_val[2]);
00113         printf("%0.3d, %0.3d, %0.3d, %0.3d, %0.3d, %0.3f, %0.3f, %0.3d, %0.3d, %0.3d, %0.3d, %0.3f, %0.3f, %0.3f,", // 4:17
00114                 neom9n_thread.GetFixType(), neom9n_thread.GetNumSV(), ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height,
00115                 neom9n_thread.GethAcc(), neom9n_thread.GetvAcc(), ubxNavPVT.velN, ubxNavPVT.velE, ubxNavPVT.velD,
00116                 ubxNavPVT.gSpeed, neom9n_thread.GetHeadMot(), neom9n_thread.GetsAcc(), neom9n_thread.GetHeadAcc());
00117         static Eigen::Vector3f pos_ecef;
00118         static Eigen::Vector3f pos_enu;
00119         static Eigen::Vector3f vel_enu;
00120         // data below only gets updated if GNSS has fix, more sats than M_MIN_SATS and a new measurement is available
00121         pos_ecef = neom9n_thread.GetPosECEF();
00122         pos_enu = neom9n_thread.GetPosENU();
00123         vel_enu = neom9n_thread.GetVelENU();
00124         printf("%0.3f, %0.3f, %0.3f, ", // 18:20
00125             pos_ecef.x(), pos_ecef.y(), pos_ecef.z());
00126         printf("%0.3f, %0.3f, %0.3f, ", // 21:23
00127             pos_enu.x(), pos_enu.y(), pos_enu.z());
00128         printf("%0.3f, %0.3f, %0.3f, ", // 24:26
00129             vel_enu.x(), vel_enu.y(), vel_enu.z());
00130         printf("%d", run_elapsed_time_ms);
00131 
00132         printf("%d\r\n", neom9n_thread.GetGPSTimeOfWeek()); // 27
00133 
00134         user_led = !user_led;
00135 
00136         // read timer and make the main thread sleep for the remaining time span (non blocking)
00137         int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();  
00138         thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
00139     }
00140 }
00141 
00142 void user_button_pressed_fcn()
00143 {
00144     user_button_timer.start();
00145     user_button_timer.reset();
00146 }
00147 
00148 void user_button_released_fcn()
00149 {
00150     // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
00151     int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
00152     user_button_timer.stop();
00153     if (user_button_elapsed_time_ms > 200) {
00154         do_execute_main_task = !do_execute_main_task;
00155     }
00156 }