AP_GPS: save configuration to non-volatile memory in UBlox GPS

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -56,7 +56,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
     next_fix(AP_GPS::NO_FIX),
     rate_update_step(0),
     _last_5hz_time(0),
-    noReceivedHdop(true)
+    noReceivedHdop(true),
+    _cfg_saved(false)
 {
     // stop any config strings that are pending
     gps.send_blob_start(state.instance, NULL, 0);
@@ -156,6 +157,8 @@ AP_GPS_UBLOX::read(void)
 
     if (need_rate_update) {
         send_next_rate_update();
+    }else if(!_cfg_saved) {         //save the configuration sent until now
+        _save_cfg();
     }
 
     numc = port->available();
@@ -395,6 +398,10 @@ AP_GPS_UBLOX::_parse_gps(void)
 {
     if (_class == CLASS_ACK) {
         Debug("ACK %u", (unsigned)_msg_id);
+
+        if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) {
+            _cfg_saved = true;
+        }
         return false;
     }
 
@@ -423,6 +430,7 @@ AP_GPS_UBLOX::_parse_gps(void)
             _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
                           &_buffer.nav_settings,
                           sizeof(_buffer.nav_settings));
+            _cfg_saved = false;     //save configuration
         }
         return false;
     }
@@ -485,6 +493,7 @@ AP_GPS_UBLOX::_parse_gps(void)
             _send_message(CLASS_CFG, MSG_CFG_SBAS,
                           &_buffer.sbas,
                           sizeof(_buffer.sbas));
+            _cfg_saved = false;
         }
     }
 
@@ -786,6 +795,18 @@ AP_GPS_UBLOX::_configure_gps(void)
     _send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
 }
 
+/*
+ *
+ */
+void
+AP_GPS_UBLOX::_save_cfg()
+{
+    ubx_cfg_cfg save_cfg;
+    save_cfg.clearMask = 0;
+    save_cfg.saveMask = SAVE_CFG_ALL;
+    save_cfg.loadMask = 0;
+    _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
+}
 
 /*
   detect a Ublox GPS. Adds one byte, and returns true if the stream

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -50,6 +50,16 @@
 #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
 #define UBX_MSG_TYPES 2
 
+//Configuration Sub-Sections
+#define SAVE_CFG_IO     (1<<0)
+#define SAVE_CFG_MSG    (1<<1)
+#define SAVE_CFG_INF    (1<<2)
+#define SAVE_CFG_NAV    (1<<3)
+#define SAVE_CFG_RXM    (1<<4)
+#define SAVE_CFG_RINV   (1<<9)
+#define SAVE_CFG_ANT    (1<<10)
+#define SAVE_CFG_ALL    (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT)
+
 class AP_GPS_UBLOX : public AP_GPS_Backend
 {
 public:
@@ -281,6 +291,19 @@ private:
         } svinfo[UBLOX_MAX_RXM_RAWX_SATS];
     };
 #endif
+
+    struct PACKED ubx_ack_ack {
+        uint8_t clsID;
+        uint8_t msgID;
+    };
+
+
+    struct PACKED ubx_cfg_cfg {
+        uint32_t clearMask;
+        uint32_t saveMask;
+        uint32_t loadMask;
+    };
+
     // Receive buffer
     union PACKED {
         ubx_nav_posllh posllh;
@@ -303,6 +326,7 @@ private:
         ubx_rxm_raw rxm_raw;
         ubx_rxm_rawx rxm_rawx;
 #endif
+        ubx_ack_ack ack;
         uint8_t bytes[];
     } _buffer;
 
@@ -321,6 +345,7 @@ private:
         MSG_DOP = 0x4,
         MSG_SOL = 0x6,
         MSG_VELNED = 0x12,
+        MSG_CFG_CFG = 0x09,
         MSG_CFG_PRT = 0x00,
         MSG_CFG_RATE = 0x08,
         MSG_CFG_SET_RATE = 0x01,
@@ -376,6 +401,7 @@ private:
     // processing
     uint8_t         _fix_count;
     uint8_t         _class;
+    bool            _cfg_saved;
 
     uint32_t        _last_vel_time;
     uint32_t        _last_pos_time;
@@ -407,6 +433,7 @@ private:
     void        _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
     void		send_next_rate_update(void);
     void        _request_version(void);
+    void        _save_cfg(void);
 
     void unexpected_message(void);
     void write_logging_headers(void);