From 9352f051337c67bb3cbf1e9e4f0822420961e117 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Aug 2015 17:27:02 +1000 Subject: [PATCH] GCS_MAVLink: fixed bug setting parameters to default values in copter if you try to set RATE_RLL_D to 0 when you haven't prevviously changed it then it would set it, but would revert on the next reboot. This is because of the special case handling of a set to the "default" value. That default value is unaware of the PID constructors this fixes that behaviour by forcing a save if the parameter changes value --- libraries/GCS_MAVLink/GCS_Common.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d6fc4bda44..47b25b9010 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -556,13 +556,27 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE); key[AP_MAX_NAME_SIZE] = 0; - vp = AP_Param::set_param_by_name(key, packet.param_value, &var_type); + // find existing param so we can get the old value + vp = AP_Param::find(key, &var_type); if (vp == NULL) { return; } + float old_value = vp->cast_to_float(var_type); + + // set the value + vp->set_float(packet.param_value, var_type); + + /* + we force the save if the value is not equal to the old + value. This copes with the use of override values in + constructors, such as PID elements. Otherwise a set to the + default value which differs from the constructor value doesn't + save the change + */ + bool force_save = !is_equal(packet.param_value, old_value); // save the change - vp->save(); + vp->save(force_save); // Report back the new value if we accepted the change // we send the value we actually set, which could be