Parameter server

dsdl/uavcan/protocol/param/598.SaveErase.uavcan

@@ -0,0 +1,11 @@
+#
+# Service to control non-volatile parameter storage.
+#
+
+uint2 OPCODE_SAVE  = 0  # Save all parameters to non-volatile storage
+uint2 OPCODE_ERASE = 1  # Clear the non-volatile storage; actual parameter values may or may not be affected
+uint2 opcode
+
+---
+
+bool ok

dsdl/uavcan/protocol/param/599.GetSet.uavcan

@@ -0,0 +1,26 @@
+#
+# Get or set a parameter by name or by index.
+#
+
+# If set - parameter will be assigned this value, then the new value will be returned
+# If not set - current parameter value will be returned
+Value value
+
+# Index of the parameter starting from 0; ignored if name is nonempty
+uint8 index
+
+# Name of the parameter; always preferred over index if nonempty
+uint8[<=40] name
+
+---
+
+# Actual parameter value. For write requests it must contain the newly assigned parameter value.
+# Empty value indicates that there is no such parameter.
+Value value
+
+Value default_value  # Optional
+Value max_value      # Optional
+Value min_value      # Optional
+
+# Empty name in response indicates that there is no such parameter
+uint8[<=40] name

dsdl/uavcan/protocol/param/Value.uavcan

@@ -0,0 +1,8 @@
+#
+# Single parameter value.
+# The actual type should be detected from the available values, as described below.
+#
+
+bool[<=1] value_bool      # Preferred over int and float if ambiguous
+int64[<=1] value_int      # Preferred over float if ambiguous
+float32[<=1] value_float

libuavcan/include/uavcan/protocol/param_server.hpp

@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/SaveErase.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+
+namespace uavcan
+{
+
+class IParamManager
+{
+public:
+    typedef typename StorageType<typename protocol::param::GetSet::Response::FieldTypes::name>::Type ParamName;
+    typedef typename StorageType<typename protocol::param::GetSet::Request::FieldTypes::index>::Type ParamIndex;
+    typedef protocol::param::Value ParamValue;
+
+    virtual ~IParamManager() { }
+
+    /**
+     * Copy the parameter name to @ref out_name if it exists, otherwise do nothing.
+     */
+    virtual void getParamNameByIndex(ParamIndex index, ParamName& out_name) const = 0;
+
+    /**
+     * Assign by name if exists.
+     */
+    virtual void assignParamValue(const ParamName& name, const ParamValue& value) = 0;
+
+    /**
+     * Read by name if exists, otherwise do nothing.
+     */
+    virtual void readParamValue(const ParamName& name, ParamValue& out_value) const = 0;
+
+    /**
+     * Read param's default/max/min if available.
+     * Implementation is optional.
+     */
+    virtual void readParamDefaultMaxMin(const ParamName& name, ParamValue& out_default,
+                                        ParamValue& out_max, ParamValue& out_min) const
+    {
+        (void)name;
+        (void)out_default;
+        (void)out_max;
+        (void)out_min;
+    }
+
+    /**
+     * Save all params to non-volatile storage.
+     * @return Negative if failed.
+     */
+    virtual int saveAllParams() = 0;
+
+    /**
+     * Clear the non-volatile storage.
+     * @return Negative if failed.
+     */
+    virtual int eraseAllParams() = 0;
+};
+
+
+class ParamServer
+{
+    typedef MethodBinder<ParamServer*, void (ParamServer::*)(const protocol::param::GetSet::Request&,
+                                                             protocol::param::GetSet::Response&)> GetSetCallback;
+
+    typedef MethodBinder<ParamServer*, void (ParamServer::*)(const protocol::param::SaveErase::Request&,
+                                                             protocol::param::SaveErase::Response&)> SaveEraseCallback;
+
+    ServiceServer<protocol::param::GetSet, GetSetCallback> get_set_srv_;
+    ServiceServer<protocol::param::SaveErase, SaveEraseCallback> save_erase_srv_;
+    IParamManager* manager_;
+
+    static bool isValueNonEmpty(const protocol::param::Value& value);
+
+    void handleGetSet(const protocol::param::GetSet::Request& request, protocol::param::GetSet::Response& response);
+
+    void handleSaveErase(const protocol::param::SaveErase::Request& request,
+                         protocol::param::SaveErase::Response& response);
+
+public:
+    ParamServer(INode& node)
+        : get_set_srv_(node)
+        , save_erase_srv_(node)
+        , manager_(NULL)
+    { }
+
+    int start(IParamManager* manager);
+
+    IParamManager* getParamManager() const { return manager_; }
+};
+
+}

libuavcan/src/protocol/param_server.cpp

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/param_server.hpp>
+
+namespace uavcan
+{
+
+bool ParamServer::isValueNonEmpty(const protocol::param::Value& value)
+{
+    return !value.value_bool.empty() || !value.value_int.empty() || !value.value_float.empty();
+}
+
+void ParamServer::handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out)
+{
+    assert(manager_ != NULL);
+
+    // Recover the name from index
+    if (in.name.empty())
+    {
+        manager_->getParamNameByIndex(in.index, out.name);
+        UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str());
+    }
+    else
+    {
+        out.name = in.name;
+    }
+
+    // Assign if needed, read back
+    if (isValueNonEmpty(in.value))
+    {
+        manager_->assignParamValue(out.name, in.value);
+    }
+    manager_->readParamValue(out.name, out.value);
+
+    // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about
+    if (isValueNonEmpty(out.value))
+    {
+        manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value);
+    }
+    else
+    {
+        UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str());
+        out.name.clear();
+    }
+}
+
+void ParamServer::handleSaveErase(const protocol::param::SaveErase::Request& in,
+                                  protocol::param::SaveErase::Response& out)
+{
+    assert(manager_ != NULL);
+
+    if (in.opcode == protocol::param::SaveErase::Request::OPCODE_SAVE)
+    {
+        out.ok = manager_->saveAllParams() >= 0;
+    }
+    else if (in.opcode == protocol::param::SaveErase::Request::OPCODE_ERASE)
+    {
+        out.ok = manager_->eraseAllParams() >= 0;
+    }
+    else
+    {
+        UAVCAN_TRACE("ParamServer", "SaveErase: invalid opcode %i", int(in.opcode));
+        out.ok = false;
+    }
+}
+
+int ParamServer::start(IParamManager* manager)
+{
+    if (manager == NULL)
+    {
+        return -1;
+    }
+    manager_ = manager;
+
+    int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet));
+    if (res < 0)
+    {
+        return res;
+    }
+
+    res = save_erase_srv_.start(SaveEraseCallback(this, &ParamServer::handleSaveErase));
+    if (res < 0)
+    {
+        get_set_srv_.stop();
+    }
+    return res;
+}
+
+}

libuavcan/test/protocol/param_server.cpp

@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <map>
+#include <gtest/gtest.h>
+#include <uavcan/protocol/param_server.hpp>
+#include "helpers.hpp"
+
+struct ParamServerTestManager : public uavcan::IParamManager
+{
+    typedef std::map<std::string, double> KeyValue;
+    KeyValue kv;
+
+    virtual void getParamNameByIndex(ParamIndex index, ParamName& out_name) const
+    {
+        ParamIndex current_idx = 0;
+        for (KeyValue::const_iterator it = kv.begin(); it != kv.end(); ++it, ++current_idx)
+        {
+            if (current_idx == index)
+            {
+                out_name = it->first.c_str();
+                break;
+            }
+        }
+    }
+
+    virtual void assignParamValue(const ParamName& name, const ParamValue& value)
+    {
+        std::cout << "ASSIGN [" << name.c_str() << "]\n" << value << "\n---" << std::endl;
+        KeyValue::iterator it = kv.find(name.c_str());
+        if (it != kv.end())
+        {
+            if (!value.value_bool.empty())
+            {
+                it->second = double(value.value_bool[0]);
+            }
+            else if (!value.value_int.empty())
+            {
+                it->second = double(value.value_int[0]);
+            }
+            else if (!value.value_float.empty())
+            {
+                it->second = double(value.value_float[0]);
+            }
+            else
+            {
+                assert(0);
+            }
+        }
+    }
+
+    virtual void readParamValue(const ParamName& name, ParamValue& out_value) const
+    {
+        KeyValue::const_iterator it = kv.find(name.c_str());
+        if (it != kv.end())
+        {
+            out_value.value_float.push_back(it->second);
+        }
+        std::cout << "READ [" << name.c_str() << "]\n" << out_value << "\n---" << std::endl;
+    }
+
+    virtual int saveAllParams()
+    {
+        std::cout << "SAVE" << std::endl;
+        return 0;
+    }
+
+    virtual int eraseAllParams()
+    {
+        std::cout << "ERASE" << std::endl;
+        return 0;
+    }
+};
+
+
+template <typename Client, typename Message>
+static void doCall(Client& client, const Message& request, InterlinkedTestNodesWithSysClock& nodes)
+{
+    ASSERT_LE(0, client.call(1, request));
+    ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1)));
+    ASSERT_TRUE(client.collector.result->isSuccessful());
+}
+
+
+TEST(ParamServer, Basic)
+{
+    InterlinkedTestNodesWithSysClock nodes;
+
+    uavcan::ParamServer server(nodes.a);
+
+    ParamServerTestManager mgr;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::param::GetSet> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::param::SaveErase> _reg2;
+
+    ASSERT_LE(0, server.start(&mgr));
+
+    ServiceClientWithCollector<uavcan::protocol::param::GetSet> get_set_cln(nodes.b);
+    ServiceClientWithCollector<uavcan::protocol::param::SaveErase> save_erase_cln(nodes.b);
+
+    /*
+     * Save/erase
+     */
+    uavcan::protocol::param::SaveErase::Request save_erase_rq;
+    save_erase_rq.opcode = uavcan::protocol::param::SaveErase::Request::OPCODE_SAVE;
+    doCall(save_erase_cln, save_erase_rq, nodes);
+    ASSERT_TRUE(save_erase_cln.collector.result->response.ok);
+
+    save_erase_rq.opcode = uavcan::protocol::param::SaveErase::Request::OPCODE_ERASE;
+    doCall(save_erase_cln, save_erase_rq, nodes);
+    ASSERT_TRUE(save_erase_cln.collector.result->response.ok);
+
+    // Invalid opcode
+    save_erase_rq.opcode = 0xFF;
+    doCall(save_erase_cln, save_erase_rq, nodes);
+    ASSERT_FALSE(save_erase_cln.collector.result->response.ok);
+
+    /*
+     * Get/set
+     */
+    uavcan::protocol::param::GetSet::Request get_set_rq;
+    get_set_rq.name = "nonexistent_parameter";
+    doCall(get_set_cln, get_set_rq, nodes);
+    ASSERT_TRUE(get_set_cln.collector.result->response.name.empty());
+
+    // No such variable, shall return empty name/value
+    get_set_rq.index = 0;
+    get_set_rq.name.clear();
+    get_set_rq.value.value_int.push_back(0xDEADBEEF);
+    doCall(get_set_cln, get_set_rq, nodes);
+    ASSERT_TRUE(get_set_cln.collector.result->response.name.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_float.empty());
+
+    mgr.kv["foobar"] = 123.456;    // New param
+
+    // Get by name
+    get_set_rq = uavcan::protocol::param::GetSet::Request();
+    get_set_rq.name = "foobar";
+    doCall(get_set_cln, get_set_rq, nodes);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
+    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty());
+    ASSERT_FLOAT_EQ(123.456, get_set_cln.collector.result->response.value.value_float[0]);
+
+    // Set by index
+    get_set_rq = uavcan::protocol::param::GetSet::Request();
+    get_set_rq.index = 0;
+    get_set_rq.value.value_int.push_back(424242);
+    doCall(get_set_cln, get_set_rq, nodes);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
+    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]);
+
+    // Get by index
+    get_set_rq = uavcan::protocol::param::GetSet::Request();
+    get_set_rq.index = 0;
+    doCall(get_set_cln, get_set_rq, nodes);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
+    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]);
+}