AP_GPS: Allow GPS's to run functions on arming (used by SBF)

Supports starting/stopping GPS logging

libraries/AP_GPS/AP_GPS.cpp

@@ -132,8 +132,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
 
     // @Param: RAW_DATA
     // @DisplayName: Raw data logging
-    // @Description: Enable logging of RXM raw data from uBlox which includes carrier phase and pseudo range information. This allows for post processing of dataflash logs for more precise positioning. Note that this requires a raw capable uBlox such as the 6P or 6T.
-    // @Values: 0:Disabled,1:log every sample,5:log every 5 samples
+    // @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into dataflash log; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming
+    // @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only)
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
@@ -1502,3 +1502,13 @@ bool AP_GPS::is_healthy(uint8_t instance) const {
            last_message_delta_time_ms(instance) < GPS_MAX_DELTA_MS &&
            drivers[instance]->is_healthy();
 }
+
+bool AP_GPS::prepare_for_arming(void) {
+    bool all_passed = true;
+    for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+        if (drivers[i] != nullptr) {
+            all_passed &= drivers[i]->prepare_for_arming();
+        }
+    }
+    return all_passed;
+}

libraries/AP_GPS/AP_GPS.h

@@ -413,6 +413,9 @@ public:
     bool is_healthy(uint8_t instance) const;
     bool is_healthy(void) const { return is_healthy(primary_instance); }
 
+    // returns true if all GPS instances have passed all final arming checks/state changes
+    bool prepare_for_arming(void);
+
 protected:
 
     // configuration parameters

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -64,32 +64,45 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 bool
 AP_GPS_SBF::read(void)
 {
-    uint32_t now = AP_HAL::millis();
-
-    if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
-        _init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
-        const char *init_str = _initialisation_blob[_init_blob_index];
-
-        if (now > _init_blob_time) {
-            if (now > _config_last_ack_time + 2500) {
-                // try to enable input on the GPS port if we have not made progress on configuring it
-                Debug("SBF Sending port enable");
-                port->write((const uint8_t*)_port_enable, strlen(_port_enable));
-                _config_last_ack_time = now;
-            } else {
-                Debug("SBF sending init string: %s", init_str);
-                port->write((const uint8_t*)init_str, strlen(init_str));
-            }
-            _init_blob_time = now + 1000;
-        }
-    }
-
     bool ret = false;
-    while (port->available() > 0) {
+    uint32_t available_bytes = port->available();
+    for (uint32_t i = 0; i < available_bytes; i++) {
         uint8_t temp = port->read();
         ret |= parse(temp);
     }
 
+    if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
+        if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
+            uint32_t now = AP_HAL::millis();
+            const char *init_str = _initialisation_blob[_init_blob_index];
+
+            if (now > _init_blob_time) {
+                if (now > _config_last_ack_time + 2500) {
+                    // try to enable input on the GPS port if we have not made progress on configuring it
+                    Debug("SBF Sending port enable");
+                    port->write((const uint8_t*)_port_enable, strlen(_port_enable));
+                    _config_last_ack_time = now;
+                } else {
+                    Debug("SBF sending init string: %s", init_str);
+                    port->write((const uint8_t*)init_str, strlen(init_str));
+                }
+                _init_blob_time = now + 1000;
+            }
+        } else if (gps._raw_data == 2) { // only manage disarm/rearms when the user opts into it
+            if (hal.util->get_soft_armed()) {
+                _has_been_armed = true;
+            } else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) {
+                // since init is done at this point and unmounting should be rate limited,
+                // take over the _init_blob_time variable
+                uint32_t now = AP_HAL::millis();
+                if (now > _init_blob_time) {
+                    unmount_disk();
+                    _init_blob_time = now + 1000;
+                }
+            }
+        }
+    }
+
     return ret;
 }
 
@@ -380,18 +393,6 @@ AP_GPS_SBF::process_message(void)
 
 void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
 {
-    if (gps._raw_data) {
-        if (!(RxState & SBF_DISK_MOUNTED)){
-            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
-        }
-        else if (RxState & SBF_DISK_FULL) {
-            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
-        }
-        else if (!(RxState & SBF_DISK_ACTIVITY)) {
-            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
-        }
-    }
-
     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
         _init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
         gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not fully configured (%d/%d)", state.instance + 1,
@@ -400,11 +401,49 @@ void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
 }
 
 bool AP_GPS_SBF::is_configured (void) {
-    return (!gps._raw_data || (RxState & SBF_DISK_ACTIVITY)) &&
-            (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
+    return (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
              _init_blob_index >= ARRAY_SIZE(_initialisation_blob));
 }
 
 bool AP_GPS_SBF::is_healthy (void) const {
     return (RxError & RX_ERROR_MASK) == 0;
 }
+
+void AP_GPS_SBF::mount_disk (void) const {
+    const char* command = "emd, DSK1, Mount\n";
+    Debug("Mounting disk");
+    port->write((const uint8_t*)command, strlen(command));
+}
+
+void AP_GPS_SBF::unmount_disk (void) const {
+    const char* command = "emd, DSK1, Unmount\n";
+    Debug("Unmounting disk");
+    port->write((const uint8_t*)command, strlen(command));
+}
+
+bool AP_GPS_SBF::prepare_for_arming(void) {
+    bool is_logging = true; // assume that its logging until proven otherwise
+    if (gps._raw_data) {
+        if (!(RxState & SBF_DISK_MOUNTED)){
+            is_logging = false;
+            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
+
+            // simply attempt to mount the disk, no need to check if the command was
+            // ACK/NACK'd as we don't continously attempt to remount the disk
+            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: Attempting to mount disk", state.instance + 1);
+            mount_disk();
+            // reset the flag to indicate if we should be logging
+            _has_been_armed = false;
+        }
+        else if (RxState & SBF_DISK_FULL) {
+            is_logging = false;
+            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
+        }
+        else if (!(RxState & SBF_DISK_ACTIVITY)) {
+            is_logging = false;
+            gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
+        }
+    }
+
+    return is_logging;
+}

libraries/AP_GPS/AP_GPS_SBF.h

@@ -48,6 +48,8 @@ public:
 
     bool is_healthy(void) const override;
 
+    bool prepare_for_arming(void) override;
+
 
 private:
 
@@ -74,6 +76,10 @@ private:
     uint32_t RxState;
     uint32_t RxError;
 
+    void mount_disk(void) const;
+    void unmount_disk(void) const;
+    bool _has_been_armed;
+
     enum sbf_ids {
         DOP = 4001,
         PVTGeodetic = 4007,

libraries/AP_GPS/GPS_Backend.h

@@ -63,6 +63,8 @@ public:
     void broadcast_gps_type() const;
     virtual void Write_DataFlash_Log_Startup_messages() const;
 
+    virtual bool prepare_for_arming(void) { return true; }
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)