Add support for unsetting parameters (track default values again)

Tools/autotest/arduplane.py

@@ -5318,6 +5318,34 @@ def SagetechMXS(self):
         home = self.home_position_as_mav_location()
         self.assert_distance(home, adsb_vehicle_loc, 0, 10000)
 
+    def unset_parameter(self, name):
+        self.mav.mav.param_unset_send(
+            self.sysid_thismav(),
+            1,
+            name.encode('ascii')
+        )
+        self.wait_message_field_values('PARAM_VALUE', {
+            'param_id': name,
+        })
+
+    def ParamUnset(self):
+        '''check unsetting of parameters'''
+        old_values = self.get_parameters(['AHRS_EKF_TYPE', 'LOG_BITMASK'])
+        self.set_parameters({
+            'AHRS_EKF_TYPE': 37,
+            'LOG_BITMASK': 2,
+        })
+        self.unset_parameter('AHRS_EKF_TYPE')
+        self.assert_parameter_values({
+            'AHRS_EKF_TYPE': old_values['AHRS_EKF_TYPE'],
+            'LOG_BITMASK': 2,
+        })
+        self.unset_parameter('LOG_BITMASK')
+        self.assert_parameter_values({
+            'AHRS_EKF_TYPE': old_values['AHRS_EKF_TYPE'],
+            'LOG_BITMASK': old_values['LOG_BITMASK'],
+        })
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -5425,6 +5453,7 @@ def tests(self):
             self.TerrainRally,
             self.MAV_CMD_NAV_LOITER_UNLIM,
             self.MAV_CMD_NAV_RETURN_TO_LAUNCH,
+            self.ParamUnset,
         ])
         return ret
 

libraries/AP_Param/AP_Param.cpp

@@ -1112,7 +1112,7 @@ void AP_Param::notify() const {
 /*
   Save the variable to HAL storage, synchronous version
 */
-void AP_Param::save_sync(bool force_save, bool send_to_gcs)
+void AP_Param::save_sync(bool force_save, bool send_to_gcs, bool unset)
 {
     uint32_t group_element = 0;
     const struct GroupInfo *ginfo;
@@ -1164,7 +1164,23 @@ void AP_Param::save_sync(bool force_save, bool send_to_gcs)
     uint16_t ofs;
     if (scan(&phdr, &ofs)) {
         // found an existing copy of the variable
-        eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
+        if (unset) {
+            // adjust the header
+            struct Param_header tmp = phdr;
+            set_key(tmp, _unset_key);
+            eeprom_write_check((uint8_t*)&tmp, ofs, sizeof(tmp));
+
+            // reset the in-memory value:
+            if (ginfo != nullptr) {
+                set_value((enum ap_var_type)phdr.type, this, get_default_value(this, *ginfo));
+            } else {
+                set_value((enum ap_var_type)phdr.type, this, get_default_value(this, *info));
+            }
+
+        } else {
+            // adjust the value
+            eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
+        }
         if (send_to_gcs) {
             send_parameter(name, (enum ap_var_type)phdr.type, idx);
         }
@@ -1173,6 +1189,10 @@ void AP_Param::save_sync(bool force_save, bool send_to_gcs)
     if (ofs == (uint16_t) ~0) {
         return;
     }
+    if (unset) {
+        // can't unset a variable we didn't find
+        return;
+    }
 
     // if the value is the default value then don't save
     if (phdr.type <= AP_PARAM_FLOAT) {
@@ -1222,9 +1242,18 @@ void AP_Param::save_sync(bool force_save, bool send_to_gcs)
 */
 void AP_Param::save(bool force_save)
 {
-    struct param_save p, p2;
-    p.param = this;
-    p.force_save = force_save;
+    const struct param_save p {
+        this,
+        true,  // force save
+        false  // don't unset
+    };
+
+    enqueue_param_save(p, force_save);
+}
+
+void AP_Param::enqueue_param_save(const struct param_save &p, bool force_save)
+{
+    struct param_save p2;
     if (save_queue.peek(p2) &&
         p2.param == this &&
         p2.force_save == force_save) {
@@ -1250,14 +1279,25 @@ void AP_Param::save(bool force_save)
     }
 }
 
+void AP_Param::unset()
+{
+    const struct param_save p {
+        this,
+        true,  // force-set
+        true  // unset
+    };
+
+    enqueue_param_save(p, true);
+}
+
 /*
   background function for saving parameters. This runs on the IO thread
  */
 void AP_Param::save_io_handler(void)
 {
     struct param_save p;
     while (save_queue.pop(p)) {
-        p.param->save_sync(p.force_save, true);
+        p.param->save_sync(p.force_save, true, p.unset);
     }
     if (hal.scheduler->is_system_initialized()) {
         // pay the cost of parameter counting in the IO thread
@@ -1343,7 +1383,8 @@ bool AP_Param::load(void)
     return true;
 }
 
-bool AP_Param::configured_in_storage(void) const
+// returns the offset of the phdr for this variable in eeprom:
+bool AP_Param::find_offset_in_storage(uint16_t &ofs) const
 {
     uint32_t group_element = 0;
     const struct GroupInfo *ginfo;
@@ -1367,12 +1408,17 @@ bool AP_Param::configured_in_storage(void) const
     phdr.group_element = group_element;
 
     // scan EEPROM to find the right location
-    uint16_t ofs;
-
     // only vector3f can have non-zero idx for now
     return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0);
 }
 
+bool AP_Param::configured_in_storage(void) const
+{
+    uint16_t ofs;
+
+    return find_offset_in_storage(ofs);
+}
+
 bool AP_Param::configured_in_defaults_file(bool &read_only) const
 {
     if (num_param_overrides == 0) {

libraries/AP_Param/AP_Param.h

@@ -400,7 +400,7 @@ class AP_Param
     ///
     /// @return                True if the variable was saved successfully.
     ///
-    void save_sync(bool force_save, bool send_to_gcs);
+    void save_sync(bool force_save, bool send_to_gcs, bool unset=false);
 
     /// flush all pending parameter saves
     /// used on reboot
@@ -448,6 +448,8 @@ class AP_Param
     */
     void set_float(float value, enum ap_var_type var_type);
 
+    void unset();
+
     // load default values for scalars in a group
     static void         setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info);
 
@@ -634,6 +636,8 @@ class AP_Param
     static const uint8_t        _sentinal_type  = 0x1F;
     static const uint8_t        _sentinal_group = 0xFF;
 
+    static const uint16_t       _unset_key   = 0x1FE;
+
     static uint16_t             _frame_type_flags;
 
     /*
@@ -759,6 +763,9 @@ class AP_Param
 
     // return true if the parameter is configured in EEPROM/FRAM
     bool configured_in_storage(void) const;
+    // return true if parameter is configured in storage, and store
+    // location in supplied offset reference
+    bool find_offset_in_storage(uint16_t &offset) const;
 
     // send a parameter to all GCS instances
     void send_parameter(const char *name, enum ap_var_type param_header_type, uint8_t idx) const;
@@ -812,9 +819,11 @@ class AP_Param
     struct PACKED param_save {
         AP_Param *param;
         bool force_save;
+        bool unset;
     };
     static ObjectBuffer_TS<struct param_save> save_queue;
     static bool registered_save_handler;
+    void enqueue_param_save(const struct param_save &p, bool force_save);
 
     // background function for saving parameters
     void save_io_handler(void);

libraries/GCS_MAVLink/GCS.h

@@ -560,6 +560,7 @@ class GCS_MAVLINK
 
     void handle_common_param_message(const mavlink_message_t &msg);
     void handle_param_set(const mavlink_message_t &msg);
+    void handle_param_unset(const mavlink_message_t &msg);
     void handle_param_request_list(const mavlink_message_t &msg);
     void handle_param_request_read(const mavlink_message_t &msg);
     virtual bool params_ready() const { return true; }

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3956,6 +3956,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
 
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
     case MAVLINK_MSG_ID_PARAM_SET:
+    case MAVLINK_MSG_ID_PARAM_UNSET:
     case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
         handle_common_param_message(msg);
         break;

libraries/GCS_MAVLink/GCS_Param.cpp

@@ -323,6 +323,60 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg)
 #endif
 }
 
+void GCS_MAVLINK::handle_param_unset(const mavlink_message_t &msg)
+{
+    mavlink_param_unset_t packet;
+    mavlink_msg_param_unset_decode(&msg, &packet);
+    enum ap_var_type var_type;
+
+    // set parameter
+    char key[AP_MAX_NAME_SIZE+1];
+    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
+    key[AP_MAX_NAME_SIZE] = 0;
+
+    // find existing param so we can get the old value
+    uint16_t parameter_flags = 0;
+    AP_Param *vp = AP_Param::find(key, &var_type, &parameter_flags);
+    if (vp == nullptr) {
+        return;
+    }
+
+    float old_value = vp->cast_to_float(var_type);
+
+    if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
+        // the user can set BRD_OPTIONS to enable set of internal
+        // parameters, for developer testing or unusual use cases
+        if (AP_BoardConfig::allow_set_internal_parameters()) {
+            parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
+        }
+    }
+
+    if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key);
+        // send the readonly value
+        send_parameter_value(key, var_type, old_value);
+        return;
+    }
+
+    // unset the value
+    vp->unset();
+
+    // tell everyone the good news:
+    const float new_value = vp->cast_to_float(var_type);
+    send_parameter_value(key, var_type, new_value);
+
+    if (parameter_flags & AP_PARAM_FLAG_ENABLE) {
+        AP_Param::invalidate_count();
+    }
+
+#if HAL_LOGGING_ENABLED
+    AP_Logger *logger = AP_Logger::get_singleton();
+    if (logger != nullptr) {
+        logger->Write_Parameter(key, vp->cast_to_float(var_type));
+    }
+#endif
+}
+
 void GCS_MAVLINK::send_parameter_value(const char *param_name, ap_var_type param_type, float param_value)
 {
     if (!HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) {
@@ -465,6 +519,9 @@ void GCS_MAVLINK::handle_common_param_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_PARAM_SET:
         handle_param_set(msg);
         break;
+    case MAVLINK_MSG_ID_PARAM_UNSET:
+        handle_param_unset(msg);
+        break;
     case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
         handle_param_request_read(msg);
         break;