AP_Periph: add support for logging UBX_TIM_TM2 message

CubePilot · May 10, 2024 · 369ea19 · 369ea19
1 parent 4a96179
commit 369ea19
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 1 deletion.
diff --git a/AP_Periph/GPS_Rover.cpp b/AP_Periph/GPS_Rover.cpp
@@ -1,6 +1,7 @@
 #include "AP_Periph.h"
 #include <AP_Filesystem/AP_Filesystem.h>
 #include <stdio.h>
+#include <hal.h>
 
 #ifdef ENABLE_BASE_MODE
 // table of user settable parameters
@@ -11,9 +12,17 @@ const AP_Param::GroupInfo GPS_Rover::var_info[] = {
     // @User: Standard
     AP_GROUPINFO_FLAGS("_ENABLE",  1, GPS_Rover, _enabled, 0, AP_PARAM_FLAG_ENABLE),
 
+    // @Param: DEBUG
+    // @DisplayName: Print Debug Messages
+    // @Description: Print Debug Messages
+    // @User: Standard
+    AP_GROUPINFO("_DEBUG",  2, GPS_Rover, _debug, 0),
+
     AP_GROUPEND
 };
 
+extern const AP_HAL::HAL &hal;
+
 GPS_Rover::GPS_Rover() {
     // setup parameters
     AP_Param::setup_object_defaults(this, var_info);
@@ -23,6 +32,7 @@ void GPS_Rover::init() {
     if (!_enabled) {
         return;
     }
+    can_printf("GPS Rover Mode Enabled\n");
     // set gps raw data if not set
     float value;
     AP_Param::get("GPS_RAW_DATA", value);
@@ -31,6 +41,21 @@ void GPS_Rover::init() {
     }
     // set callback for GPS RAW data
     AP::gps().set_gps_raw_cb(FUNCTOR_BIND_MEMBER(&GPS_Rover::ubx_rawdata_cb, void, const uint8_t*, uint32_t));
+
+    // Set PA13 and PA14 mode as input
+    palSetLineMode(HAL_GPIO_PIN_JTCK_SWCLK, PAL_MODE_INPUT);
+    palSetLineMode(HAL_GPIO_PIN_JTMS_SWDIO, PAL_MODE_INPUT);
+
+    // set callback for EXTERN_GPIO1
+    hal.gpio->attach_interrupt(EXTERN_GPIO1, FUNCTOR_BIND_MEMBER(&GPS_Rover::handle_feedback, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH);
+}
+
+void GPS_Rover::handle_feedback(uint8_t pin, bool pin_state, uint32_t timestamp)
+{
+    // trigger EXTINT
+    if (pin == EXTERN_GPIO1) {
+        hal.gpio->write(GPS_EXTINT, pin_state);
+    }
 }
 
 void GPS_Rover::update() {
@@ -42,7 +67,7 @@ void GPS_Rover::update() {
 }
 
 void GPS_Rover::ubx_rawdata_cb(const uint8_t *data, uint32_t length) {
-    if (ubx_log_fd == -1) {
+    if (ubx_log_fd == -1 && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) && AP::gps().time_week()) {    
         struct stat st;
         int ret = AP::FS().stat("rover", &st);
         if (ret == -1) {
@@ -63,6 +88,10 @@ void GPS_Rover::ubx_rawdata_cb(const uint8_t *data, uint32_t length) {
     }
 
     if (ubx_log_fd != -1) {
+        // check if UBX_TIM_TM2 is received
+        if ((data[2] == 0x0D) && (data[3] == 0x03) && _debug) {
+            can_printf("Received UBX_TIM_TM2!\n");
+        }
         AP::FS().write(ubx_log_fd, data, length);
         AP::FS().fsync(ubx_log_fd);
     }

diff --git a/AP_Periph/GPS_Rover.h b/AP_Periph/GPS_Rover.h
@@ -9,9 +9,11 @@ class GPS_Rover {
     static const struct AP_Param::GroupInfo var_info[];
 private:
     void init();
+    void handle_feedback(uint8_t pin, bool pin_state, uint32_t timestamp);
     void ubx_rawdata_cb(const uint8_t *data, uint32_t length);
     bool _initialized;
     AP_Int8 _enabled;
+    AP_Int8 _debug;
     int ubx_log_fd = -1;
     char _ubx_log_filename[48];
 };