From 0da3a93ec9a88a6f042c134df38348d8264c3003 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 25 Mar 2014 19:16:56 +0400 Subject: [PATCH] Parameter server --- .../protocol/param/598.SaveErase.uavcan | 11 ++ dsdl/uavcan/protocol/param/599.GetSet.uavcan | 26 +++ dsdl/uavcan/protocol/param/Value.uavcan | 8 + .../include/uavcan/protocol/param_server.hpp | 97 +++++++++++ libuavcan/src/protocol/param_server.cpp | 94 ++++++++++ libuavcan/test/protocol/param_server.cpp | 163 ++++++++++++++++++ 6 files changed, 399 insertions(+) create mode 100644 dsdl/uavcan/protocol/param/598.SaveErase.uavcan create mode 100644 dsdl/uavcan/protocol/param/599.GetSet.uavcan create mode 100644 dsdl/uavcan/protocol/param/Value.uavcan create mode 100644 libuavcan/include/uavcan/protocol/param_server.hpp create mode 100644 libuavcan/src/protocol/param_server.cpp create mode 100644 libuavcan/test/protocol/param_server.cpp diff --git a/dsdl/uavcan/protocol/param/598.SaveErase.uavcan b/dsdl/uavcan/protocol/param/598.SaveErase.uavcan new file mode 100644 index 0000000000..4d4d933fd4 --- /dev/null +++ b/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 diff --git a/dsdl/uavcan/protocol/param/599.GetSet.uavcan b/dsdl/uavcan/protocol/param/599.GetSet.uavcan new file mode 100644 index 0000000000..73f5d74dab --- /dev/null +++ b/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 diff --git a/dsdl/uavcan/protocol/param/Value.uavcan b/dsdl/uavcan/protocol/param/Value.uavcan new file mode 100644 index 0000000000..083db54bdf --- /dev/null +++ b/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 diff --git a/libuavcan/include/uavcan/protocol/param_server.hpp b/libuavcan/include/uavcan/protocol/param_server.hpp new file mode 100644 index 0000000000..3fb5ff3d4d --- /dev/null +++ b/libuavcan/include/uavcan/protocol/param_server.hpp @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan +{ + +class IParamManager +{ +public: + typedef typename StorageType::Type ParamName; + typedef typename StorageType::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 GetSetCallback; + + typedef MethodBinder SaveEraseCallback; + + ServiceServer get_set_srv_; + ServiceServer 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_; } +}; + +} diff --git a/libuavcan/src/protocol/param_server.cpp b/libuavcan/src/protocol/param_server.cpp new file mode 100644 index 0000000000..11e54eb421 --- /dev/null +++ b/libuavcan/src/protocol/param_server.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +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; +} + +} diff --git a/libuavcan/test/protocol/param_server.cpp b/libuavcan/test/protocol/param_server.cpp new file mode 100644 index 0000000000..0f90893344 --- /dev/null +++ b/libuavcan/test/protocol/param_server.cpp @@ -0,0 +1,163 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +struct ParamServerTestManager : public uavcan::IParamManager +{ + typedef std::map 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 +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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + ASSERT_LE(0, server.start(&mgr)); + + ServiceClientWithCollector get_set_cln(nodes.b); + ServiceClientWithCollector 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]); +}