AP_Periph: add support for rover side logging

Author: bugobliterator

AP_Periph/AP_Periph.cpp

@@ -257,6 +257,7 @@ void AP_Periph_FW::update()
 
 #ifdef ENABLE_BASE_MODE
     gps_base.update();
+    gps_rover.update();
 #endif
 
     SRV_Channels::enable_aux_servos();

AP_Periph/AP_Periph.h

@@ -33,6 +33,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include "MAVLink.h"
 #include "GPS_Base.h"
+#include "GPS_Rover.h"
 
 #if defined(HAL_PERIPH_ENABLE_BATTERY_MPPT_PACKETDIGITAL) && HAL_MAX_CAN_PROTOCOL_DRIVERS < 2
 #error "Battery MPPT PacketDigital driver requires at least two CAN Ports"
@@ -230,6 +231,7 @@ class AP_Periph_FW {
 
 #ifdef ENABLE_BASE_MODE
     GPS_Base gps_base;
+    GPS_Rover gps_rover;
 #endif
 
 #ifdef GPIO_UBX_SAFEBOOT

AP_Periph/GPS_Base.cpp

@@ -122,7 +122,7 @@ GPS_Base::GPS_Base() {
 }
 
 // convert week number and time of week to date/time
-void GPS_Base::gps_week_time(const uint16_t week, const uint32_t tow)
+void GPS_Base::gps_week_time(struct date_time &dt, const uint16_t week, const uint32_t tow)
 {
     // days since 1st epoch (6th Jan 1980)
     uint32_t days = (week * 7) + (tow / 86400000) + 5;
@@ -170,15 +170,15 @@ void GPS_Base::parse_runtime_ubx(uint8_t byte) {
         // handle RAWX message
         if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX) {
             if (_buffer.raw_rawx.week > 0 && _buffer.raw_rawx.rcvTow >= 0) {
-                gps_week_time((uint16_t)_buffer.raw_rawx.week, (uint32_t)(_buffer.raw_rawx.rcvTow * 1000));
+                gps_week_time(dt, (uint16_t)_buffer.raw_rawx.week, (uint32_t)(_buffer.raw_rawx.rcvTow * 1000));
                 can_printf("GPS: %d-%02d-%02d %02d:%02d:%02d\n", dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second);
             }
         } else if (_class == CLASS_NAV && _msg_id == MSG_NAV_SVIN) {
             can_printf("GPS: Survey in status: %s Active:%d Acc:%fm\n", _buffer.nav_svin.valid ? "Valid":"Invalid", _buffer.nav_svin.active, _buffer.nav_svin.meanAcc/10000.0);
             if (_buffer.nav_svin.valid) {
                 can_printf("GPS: Survey in complete\n");
             }
-            if (_start_mean_acc == 0.0 && _buffer.nav_svin.active && (_buffer.nav_svin.meanAcc < (50*10000))) {
+            if (_start_mean_acc == 0 && _buffer.nav_svin.active && (_buffer.nav_svin.meanAcc < (50*10000))) {
                 _start_mean_acc = _buffer.nav_svin.meanAcc;
             }
             memcpy(&curr_svin, &_buffer.nav_svin, sizeof(curr_svin));
@@ -526,7 +526,7 @@ void GPS_Base::do_configurations()
             if ((AP_HAL::millis() - _last_surveyin_config_ms) > 1000) {
                 can_printf("GPS_Base: Setting survey in config");
                 ubx_cfg_tmode3 surveyin_cfg {};
-                if (_s_in_lat == 0.0 && _s_in_lon == 0.0 && _s_in_alt == 0.0) {
+                if (int(_s_in_lat*1000) == 0 && int(_s_in_lon*1000) == 0 && int(_s_in_alt*1000) == 0) {
                     surveyin_cfg.flags = 1;
                     surveyin_cfg.fixedPosAcc = 0;
                     surveyin_cfg.svinAccLimit = _s_in_acc*10000.0;

AP_Periph/GPS_Base.h

@@ -3,7 +3,15 @@
 #include <AP_GPS/RTCM3_Parser.h>
 
 #ifdef ENABLE_BASE_MODE
-
+struct date_time {
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+    uint32_t utc_sec;
+};
 class GPS_Base {
 public:
     GPS_Base();
@@ -14,9 +22,8 @@ class GPS_Base {
     bool enabled() const { return _enabled; } 
 
     static const struct AP_Param::GroupInfo var_info[];
-
+    static void gps_week_time(struct date_time &dt, const uint16_t week, const uint32_t tow);
 private:
-    void gps_week_time(const uint16_t week, const uint32_t tow);
     void parse_runtime_ubx(uint8_t byte);
     void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
     bool _send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size);
@@ -195,16 +202,7 @@ class GPS_Base {
 
     int ubx_file = -1;
 
-    struct date_time {
-        uint16_t year;
-        uint8_t month;
-        uint8_t day;
-        uint8_t hour;
-        uint8_t minute;
-        uint8_t second;
-        uint32_t utc_sec;
-    } dt;
-
+    struct date_time dt;
     bool gps_received_preamble;
     bool gcs_received_preamble;
     uint8_t gps_length_counter;

AP_Periph/GPS_Rover.cpp

@@ -0,0 +1,71 @@
+#include "AP_Periph.h"
+#include <AP_Filesystem/AP_Filesystem.h>
+#include <stdio.h>
+
+#ifdef ENABLE_BASE_MODE
+// table of user settable parameters
+const AP_Param::GroupInfo GPS_Rover::var_info[] = {
+    // @Param: ENABLE
+    // @DisplayName: Enable GPS Rover Mode
+    // @Description: Enable GPS Rover Mode
+    // @User: Standard
+    AP_GROUPINFO_FLAGS("_ENABLE",  1, GPS_Rover, _enabled, 0, AP_PARAM_FLAG_ENABLE),
+
+    AP_GROUPEND
+};
+
+GPS_Rover::GPS_Rover() {
+    // setup parameters
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void GPS_Rover::init() {
+    if (!_enabled) {
+        return;
+    }
+    // set gps raw data if not set
+    float value;
+    AP_Param::get("GPS_RAW_DATA", value);
+    if (value < 1.0) {
+        AP_Param::set_and_save_by_name("GPS_RAW_DATA", 1);
+    }
+    // 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));
+}
+
+void GPS_Rover::update() {
+    // do nothing if not enabled
+    if (!_initialized && _enabled && (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D)) {
+        init();
+        _initialized = true;
+    }
+}
+
+void GPS_Rover::ubx_rawdata_cb(const uint8_t *data, uint32_t length) {
+    if (ubx_log_fd == -1) {
+        struct stat st;
+        int ret = AP::FS().stat("rover", &st);
+        if (ret == -1) {
+            ret = AP::FS().mkdir("rover");
+        }
+        date_time dt;
+        GPS_Base::gps_week_time(dt, AP::gps().time_week(), AP::gps().time_week_ms());
+        snprintf(_ubx_log_filename, sizeof(_ubx_log_filename), "/rover/UTC_%04d_%02d_%02d_%02d_%02d_%02d.ubx", dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second);
+        ubx_log_fd = AP::FS().open(_ubx_log_filename, O_CREAT | O_WRONLY | O_TRUNC);
+        if (ubx_log_fd < 0) {
+            can_printf("Failed to create rover log file %d\n", errno);
+        } else {
+            can_printf("Opened rover log file %s\n", _ubx_log_filename);
+        }
+        if (!AP::FS().set_mtime(_ubx_log_filename, dt.utc_sec)) {
+            can_printf("Failed to set file time %s\n", strerror(errno));
+        }
+    }
+
+    if (ubx_log_fd != -1) {
+        AP::FS().write(ubx_log_fd, data, length);
+        AP::FS().fsync(ubx_log_fd);
+    }
+}
+
+#endif // ENABLE_BASE_MODE

AP_Periph/GPS_Rover.h

@@ -0,0 +1,19 @@
+#pragma once
+#include <AP_Param/AP_Param.h>
+
+#ifdef ENABLE_BASE_MODE
+class GPS_Rover {
+public:
+    GPS_Rover();
+    void update();
+    static const struct AP_Param::GroupInfo var_info[];
+private:
+    void init();
+    void ubx_rawdata_cb(const uint8_t *data, uint32_t length);
+    bool _initialized;
+    AP_Int8 _enabled;
+    int ubx_log_fd = -1;
+    char _ubx_log_filename[48];
+};
+
+#endif

AP_Periph/Parameters.cpp

@@ -206,6 +206,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(gps_safeboot, "GPS_SAFEBOOT", 0),
 #endif
 
+#ifdef ENABLE_BASE_MODE
+    // @Param: ROVER
+    // @Path: GPS_Rover.cpp
+    GOBJECT(gps_rover, "ROVER",  GPS_Rover),
+#endif
+
     AP_VAREND
 };
 

AP_Periph/Parameters.h

@@ -58,6 +58,7 @@ class Parameters {
         k_param_imu_sample_rate,
         k_param_imu,
         k_param_gps_safeboot,
+        k_param_gps_rover,
     };
 
     AP_Int16 format_version;