diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index b19a3bf831..5792dea2b3 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -75,7 +75,7 @@ static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; -static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -19}; +static constexpr wq_config_t uavcan{"wq:uavcan", 3680, -19}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 6891d84410..0829132b37 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -119,12 +119,16 @@ px4_add_module( ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include + COMPILE_OPTIONS + #-O0 SRCS allocator.hpp uavcan_driver.hpp indication_controller.cpp UavcanNode.cpp UavcanNode.hpp + UavcanNodeParamManager.hpp + UavcanNodeParamManager.cpp DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index d52e48dec6..30a60d7169 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -83,6 +83,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _raw_air_data_publisher(_node), _range_sensor_measurement(_node), _flow_measurement_publisher(_node), + _param_server(_node), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")), _reset_timer(_node) @@ -290,6 +291,7 @@ void UavcanNode::Run() if (!_initialized) { get_node().setRestartRequestHandler(&restart_request_handler); + _param_server.start(&_param_manager); init_indication_controller(get_node()); @@ -313,6 +315,7 @@ void UavcanNode::Run() _sensor_baro_sub.registerCallback(); _sensor_gps_sub.registerCallback(); _sensor_mag_sub.registerCallback(); + _sensor_gps_sub.registerCallback(); _initialized = true; } diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index aa54b21665..8b66ad5c04 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -48,6 +48,7 @@ #include "uavcan_driver.hpp" #include "allocator.hpp" +#include "UavcanNodeParamManager.hpp" #include #include @@ -200,6 +201,9 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)}; uORB::SubscriptionCallbackWorkItem _sensor_gps_sub{this, ORB_ID(sensor_gps)}; + UavcanNodeParamManager _param_manager; + uavcan::ParamServer _param_server; + perf_counter_t _cycle_perf; perf_counter_t _interval_perf; diff --git a/src/drivers/uavcannode/UavcanNodeParamManager.cpp b/src/drivers/uavcannode/UavcanNodeParamManager.cpp new file mode 100644 index 0000000000..e405e93b5f --- /dev/null +++ b/src/drivers/uavcannode/UavcanNodeParamManager.cpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "UavcanNodeParamManager.hpp" + +#include +#include + +void UavcanNodeParamManager::getParamNameByIndex(Index index, Name &out_name) const +{ + if (index < param_count_used()) { + out_name = param_name(param_for_used_index(index)); + } +} + +void UavcanNodeParamManager::assignParamValue(const Name &name, const Value &value) +{ + param_t param_handle = param_find(name.c_str()); + + if (param_handle == PARAM_INVALID) { + // Invalid parameter name + return; + } + + // Assign input value to parameter if types match + param_type_t value_type = param_type(param_handle); + + if (value.is(uavcan::protocol::param::Value::Tag::integer_value) && (value_type == PARAM_TYPE_INT32)) { + auto val = *value.as(); + + if ((val <= INT32_MAX) || (val >= INT32_MIN)) { + int32_t in_param = val; + param_set(param_handle, &in_param); + } + + } else if (value.is(uavcan::protocol::param::Value::Tag::real_value) && (value_type == PARAM_TYPE_FLOAT)) { + // TODO: min/max value checking for float + float in_param = *value.as(); + param_set(param_handle, &in_param); + } +} + +void UavcanNodeParamManager::readParamValue(const Name &name, Value &out_value) const +{ + param_t param_handle = param_find(name.c_str()); + + if (param_handle == PARAM_INVALID) { + // Invalid parameter name + return; + } + + // Copy current parameter value to out_value + param_type_t value_type = param_type(param_handle); + + if (value_type == PARAM_TYPE_INT32) { + int32_t current_value; + param_get(param_handle, ¤t_value); + out_value.to() = current_value; + + } else if (value_type == PARAM_TYPE_FLOAT) { + float current_value; + param_get(param_handle, ¤t_value); + out_value.to() = current_value; + } +} + +void UavcanNodeParamManager::readParamDefaultMaxMin(const Name &name, Value &out_default, + NumericValue &out_max, NumericValue &out_min) const +{ + // TODO: get actual default value (will require a new function in param.h) + param_t param_handle = param_find(name.c_str()); + + if (param_handle == PARAM_INVALID) { + // Invalid parameter name + return; + } + + param_type_t value_type = param_type(param_handle); + + if (value_type == PARAM_TYPE_INT32) { + out_max.to() = INT32_MAX; + out_min.to() = INT32_MIN; + out_default.to() = 0; + + } else if (value_type == PARAM_TYPE_FLOAT) { + out_max.to() = FLT_MAX; + out_min.to() = -FLT_MAX; + out_default.to() = 0.0; + } +} + +int UavcanNodeParamManager::saveAllParams() +{ + // Nothing to do here assuming autosave is turned on + return 0; +} + +int UavcanNodeParamManager::eraseAllParams() +{ + param_reset_all(); + return 0; +} diff --git a/src/drivers/uavcannode/UavcanNodeParamManager.hpp b/src/drivers/uavcannode/UavcanNodeParamManager.hpp new file mode 100644 index 0000000000..e29647ff46 --- /dev/null +++ b/src/drivers/uavcannode/UavcanNodeParamManager.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +class UavcanNodeParamManager : public uavcan::IParamManager +{ +public: + UavcanNodeParamManager() = default; + + void getParamNameByIndex(Index index, Name &out_name) const override; + void assignParamValue(const Name &name, const Value &value) override; + void readParamValue(const Name &name, Value &out_value) const override; + void readParamDefaultMaxMin(const Name &name, Value &out_default, + NumericValue &out_max, NumericValue &out_min) const override; + int saveAllParams() override; + int eraseAllParams() override; + +private: + +};