param_server: add error when params are provided too late

The mavlink parameter protocol uses an index to set/get specific
parameters. When all parameters are requested, the total count is sent
out, together with all the parameters.

Now once this overall count has been sent out, we can't add/provide
more parameters on the server side. Until now, MAVSDK didn't care, but
with this commit, we are adding a new error to signal that we can no
longer change the param list.

This implementation has a bit of a race condition unfortunately: if a
request list message is received right in the beginning when all
parameters are added, we lock the list too early. The correct fix for
this would be a specific method to "end param provisions" and lock them
down. However, requiring such a method would be a breaking change, so
I'm not adding this now but I'm considering it for the next major
version of MAVSDK.

Author: julianoes

proto

@@ -482,6 +482,10 @@ void MavlinkParameterServer::process_param_ext_request_list(const mavlink_messag
 void MavlinkParameterServer::broadcast_all_parameters(const bool extended)
 {
     std::lock_guard<std::mutex> lock(_all_params_mutex);
+
+    // Param used with index, we should no longer change the index
+    _params_locked_down = true;
+
     const auto all_params = _param_cache.all_parameters(extended);
     if (_parameter_debugging) {
         LogDebug() << "broadcast_all_parameters " << (extended ? "extended" : "") << ": "
@@ -687,4 +691,11 @@ MavlinkParameterServer::extract_request_read_param_identifier(
     }
 }
 
+bool MavlinkParameterServer::params_locked_down() const
+{
+    std::lock_guard<std::mutex> lock(_all_params_mutex);
+
+    return _params_locked_down;
+}
+
 } // namespace mavsdk

src/mavsdk/core/mavlink_parameter_server.cpp

@@ -64,6 +64,8 @@ class MavlinkParameterServer : public MavlinkParameterSubscription {
     void set_extended_protocol(bool extended_protocol) { _extended_protocol = extended_protocol; };
     void do_work();
 
+    bool params_locked_down() const;
+
     friend std::ostream& operator<<(std::ostream&, const Result&);
 
     // Non-copyable
@@ -121,14 +123,16 @@ class MavlinkParameterServer : public MavlinkParameterSubscription {
     Sender& _sender;
     MavlinkMessageHandler& _message_handler;
 
-    std::mutex _all_params_mutex{};
+    mutable std::mutex _all_params_mutex{};
     MavlinkParameterCache _param_cache{};
 
     LockedQueue<WorkItem> _work_queue{};
 
     bool _extended_protocol = true;
     bool _parameter_debugging = false;
     bool _last_extended = true;
+
+    bool _params_locked_down = false;
 };
 
 } // namespace mavsdk

src/mavsdk/core/mavlink_parameter_server.h

@@ -256,6 +256,12 @@ Action::Result Action::set_current_speed(float speed_m_s) const
     return _impl->set_current_speed(speed_m_s);
 }
 
+Action::Result Action::set_gps_global_origin(
+    double latitude_deg, double longitude_deg, float absolute_altitude_m) const
+{
+    return _impl->set_gps_global_origin(latitude_deg, longitude_deg, absolute_altitude_m);
+}
+
 std::ostream& operator<<(std::ostream& str, Action::Result const& result)
 {
     switch (result) {

src/mavsdk/plugins/action/action.cpp

@@ -644,6 +644,20 @@ class Action : public PluginBase {
      */
     Result set_current_speed(float speed_m_s) const;
 
+    /**
+     * @brief Set GPS Global Origin.
+     *
+     * Sets the GPS coordinates of the vehicle local origin (0,0,0) position.
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_gps_global_origin(
+        double latitude_deg, double longitude_deg, float absolute_altitude_m) const;
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/action/include/plugins/action/action.h

@@ -150,6 +150,7 @@ class ParamServer : public ServerPluginBase {
         ParamNameTooLong, /**< @brief Parameter name too long (> 16). */
         NoSystem, /**< @brief No system available. */
         ParamValueTooLong, /**< @brief Parameter name too long (> 128). */
+        ParamProvidedTooLate, /**< @brief All parameters have to be provided upfront. */
     };
 
     /**
@@ -195,6 +196,10 @@ class ParamServer : public ServerPluginBase {
      *
      * If the type is wrong, the result will be `WRONG_TYPE`.
      *
+     * Note that all params need to be provided upfront. Once a client has
+     * requested a param list, the indices are locked and no more params
+     * can be added.
+     *
      * This function is blocking.
      *
 
@@ -208,6 +213,10 @@ class ParamServer : public ServerPluginBase {
      *
      * If the type is wrong, the result will be `WRONG_TYPE`.
      *
+     * Note that all params need to be provided upfront. Once a client has
+     * requested a param list, the indices are locked and no more params
+     * can be added.
+     *
      * This function is blocking.
      *
      * @return Result of request.
@@ -232,6 +241,10 @@ class ParamServer : public ServerPluginBase {
      *
      * If the type is wrong, the result will be `WRONG_TYPE`.
      *
+     * Note that all params need to be provided upfront. Once a client has
+     * requested a param list, the indices are locked and no more params
+     * can be added.
+     *
      * This function is blocking.
      *
      * @return Result of request.

src/mavsdk/plugins/param_server/include/plugins/param_server/param_server.h

@@ -187,6 +187,8 @@ std::ostream& operator<<(std::ostream& str, ParamServer::Result const& result)
             return str << "No System";
         case ParamServer::Result::ParamValueTooLong:
             return str << "Param Value Too Long";
+        case ParamServer::Result::ParamProvidedTooLate:
+            return str << "Param Provided Too Late";
         default:
             return str << "Unknown";
     }

src/mavsdk/plugins/param_server/param_server.cpp

@@ -62,6 +62,11 @@ ParamServer::Result ParamServerImpl::provide_param_int(std::string name, int32_t
     if (name.size() > 16) {
         return ParamServer::Result::ParamNameTooLong;
     }
+
+    if (_server_component_impl->mavlink_parameter_server().params_locked_down()) {
+        return ParamServer::Result::ParamProvidedTooLate;
+    }
+
     const auto ret =
         _server_component_impl->mavlink_parameter_server().provide_server_param_int(name, value);
     if (ret == MavlinkParameterServer::Result::Ok) {
@@ -90,6 +95,11 @@ ParamServer::Result ParamServerImpl::provide_param_float(std::string name, float
     if (name.size() > 16) {
         return ParamServer::Result::ParamNameTooLong;
     }
+
+    if (_server_component_impl->mavlink_parameter_server().params_locked_down()) {
+        return ParamServer::Result::ParamProvidedTooLate;
+    }
+
     const auto ret =
         _server_component_impl->mavlink_parameter_server().provide_server_param_float(name, value);
     if (ret == MavlinkParameterServer::Result::Ok) {