diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ce476165310b4..eb71398434bbe 100644 --- a/Tools/autotest/arduplane.py +++ b/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 diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index b10529d15b0a0..cd82a17aa58fd 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/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,6 +1279,17 @@ 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 */ @@ -1257,7 +1297,7 @@ 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) { diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 4fed080f07e66..13f6336480a79 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/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 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); diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 415dd1b51c9e6..e42468c4a68cc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/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; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index eac5d393a07fe..f8a6bc22869ba 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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; diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index a4c8c1df5182c..771896758d5c9 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/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, ¶meter_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; diff --git a/modules/mavlink b/modules/mavlink index 130a836efbfef..45aaed4037f78 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 130a836efbfef0eb3287a92cd5e187de7facdce2 +Subproject commit 45aaed4037f78d6dfc53825fa6f1e288bd38da94