From d720a42a35afa8efac42371c065b8ab7d8a9ba6d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 3 Jun 2015 13:47:36 -1000 Subject: [PATCH] Changes to build on latest uavcan master with FW upload and Node ID allocation --- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 9 +- src/modules/uavcan/sensors/baro.cpp | 65 ++++++++----- src/modules/uavcan/sensors/baro.hpp | 26 +++-- src/modules/uavcan/uavcan_main.cpp | 142 +++++++++++++++++++++++++--- src/modules/uavcan/uavcan_main.hpp | 44 ++++++++- 6 files changed, 237 insertions(+), 51 deletions(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 7719f7692aba..1f1679c75d98 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 +Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e3fd31a9a1a7..1d6c205586c5 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 +MODULE_STACKSIZE = 3200 +WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ @@ -65,7 +67,6 @@ INCLUDE_DIRS += $(LIBUAVCAN_INC) # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS - # # libuavcan drivers for STM32 # @@ -75,6 +76,12 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 +# +# libuavcan drivers for posix +# +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/posix/include.mk +INCLUDE_DIRS += $(LIBUAVCAN_POSIX_INC) + # # Invoke DSDL compiler # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 9c3a0aefb4a1..658b3fb41cee 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -42,9 +42,11 @@ const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node), +_sub_air_pressure_data(node), +_sub_air_temperature_data(node), _reports(nullptr) { + last_temperature = 0.0f; } int UavcanBarometerBridge::init() @@ -59,11 +61,17 @@ int UavcanBarometerBridge::init() if (_reports == nullptr) return -1; - res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); - if (res < 0) { - log("failed to start uavcan sub: %d", res); - return res; - } + res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } return 0; } @@ -127,31 +135,36 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) } } -void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + last_temperature = msg.static_temperature; +} + +void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg) { - baro_report report; + baro_report report; - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = msg.static_temperature; - report.pressure = msg.static_pressure / 100.0F; // Convert to millibar - report.error_count = 0; + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + report.temperature = last_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + report.error_count = 0; - /* - * Altitude computation - * Refer to the MS5611 driver for details - */ - const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin - const double a = -6.5 / 1000; // temperature gradient in degrees per metre - const double g = 9.80665; // gravity constant in m/s/s - const double R = 287.05; // ideal gas constant in J/kg/K + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K - const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa - const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa - report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - // add to the ring buffer - _reports->force(&report); + // add to the ring buffer + _reports->force(&report); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index b5da6985a78f..4c68c497fcd9 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -41,7 +41,10 @@ #include #include -#include +#include +#include + +class RingBuffer; class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { @@ -58,14 +61,23 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); + void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirPressureCbBinder; - typedef uavcan::MethodBinder < UavcanBarometerBridge *, - void (UavcanBarometerBridge::*) - (const uavcan::ReceivedDataStructure &) > - AirDataCbBinder; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirTemperatureCbBinder; - uavcan::Subscriber _sub_air_data; + uavcan::Subscriber _sub_air_pressure_data; + uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; ringbuffer::RingBuffer *_reports; + float last_temperature; + }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8104b4b28942..aa3be610f4f8 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,25 +53,42 @@ #include #include "uavcan_main.hpp" +#include +#include + +#include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +#define OK 0 /** * @file uavcan_main.cpp * - * Implements basic functinality of UAVCAN node. + * Implements basic functionality of UAVCAN node. * * @author Pavel Kirienko + * David Sidrane */ /* * UavcanNode */ UavcanNode *UavcanNode::_instance; +uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; +uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; +uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; +uavcan_posix::FirmwareVersionChecker fw_version_checker; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), - _esc_controller(_node) + _esc_controller(_node), + _fileserver_backend(_node), + _node_info_retriever(_node), + _fw_upgrade_trigger(_node, fw_version_checker), + _fw_server(_node, _fileserver_backend) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -79,6 +96,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _control_topics[3] = ORB_ID(actuator_controls_3); const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { std::abort(); } @@ -124,6 +142,7 @@ UavcanNode::~UavcanNode() // Removing the sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { auto next = br->getSibling(); delete br; @@ -135,6 +154,8 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); + delete(_server_instance); + } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -196,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { warnx("start failed: %d", errno); @@ -228,8 +249,10 @@ void UavcanNode::fill_node_info() if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { hwver.major = 2; + } else { ; // All other values of HW_ARCH resolve to zero } @@ -241,6 +264,7 @@ void UavcanNode::fill_node_info() _node.setHardwareVersion(hwver); } + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -260,6 +284,7 @@ int UavcanNode::init(uavcan::NodeID node_id) // Actuators ret = _esc_controller.init(); + if (ret < 0) { return ret; } @@ -267,26 +292,101 @@ int UavcanNode::init(uavcan::NodeID node_id) // Sensor bridges IUavcanSensorBridge::make_all(_node, _sensor_bridges); auto br = _sensor_bridges.getHead(); + while (br != nullptr) { ret = br->init(); + if (ret < 0) { warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); return ret; } + warnx("sensor bridge '%s' init ok", br->get_name()); br = br->getSibling(); } + + /* Initialize the fw version checker. + * giving it it's path + */ + + ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); + + if (ret < 0) { + return ret; + } + + + /* Start fw file server back */ + + ret = _fw_server.start(); + + if (ret < 0) { + return ret; + } + + /* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ + + ret = storage_backend.init(UAVCAN_NODE_DB_PATH); + + if (ret < 0) { + return ret; + } + + /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ + + ret = tracer.init(UAVCAN_LOG_FILE); + + if (ret < 0) { + return ret; + } + + /* Create dynamic node id server for the Firmware updates directory */ + + _server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer); + + if (_server_instance == 0) { + return -ENOMEM; + } + + /* Initialize the dynamic node id server */ + ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id); + + if (ret < 0) { + return ret; + } + + /* Start node info retriever to fetch node info from new nodes */ + + ret = _node_info_retriever.start(); + + if (ret < 0) { + return ret; + } + + + /* Start the fw version checker */ + + ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath()); + + if (ret < 0) { + return ret; + } + + /* Start the Node */ + return _node.start(); } void UavcanNode::node_spin_once() { perf_begin(_perfcnt_node_spin_elapsed); - const int spin_res = _node.spin(uavcan::MonotonicTime()); + const int spin_res = _node.spinOnce(); + if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } @@ -297,9 +397,11 @@ void UavcanNode::node_spin_once() int UavcanNode::add_poll_fd(int fd) { int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { errx(1, "uavcan: too many poll fds, exiting"); } + _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = fd; _poll_fds[_poll_fds_num].events = POLLIN; @@ -312,8 +414,6 @@ int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); - const unsigned PollTimeoutMs = 50; - // XXX figure out the output count _output_count = 2; @@ -324,6 +424,7 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); + if (busevent_fd < 0) { warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); @@ -354,6 +455,7 @@ int UavcanNode::run() } while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -379,9 +481,11 @@ int UavcanNode::run() if (poll_ret < 0) { log("poll error %d", errno); continue; + } else { // get controls for required topics bool controls_updated = false; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { @@ -401,8 +505,9 @@ int UavcanNode::run() if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], - _actuator_direct.nvalues*sizeof(float)); + _actuator_direct.nvalues * sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; } @@ -411,10 +516,12 @@ int UavcanNode::run() if (_test_in_progress) { memset(&_outputs, 0, sizeof(_outputs)); if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - _outputs.noutputs = _test_motor.motor_number+1; + _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; + _outputs.noutputs = _test_motor.motor_number + 1; } + new_output = true; + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -451,6 +558,7 @@ int UavcanNode::run() _outputs.output[i] = 1.0f; } } + // Output to the bus _outputs.timestamp = hrt_absolute_time(); perf_begin(_perfcnt_esc_mixer_output_elapsed); @@ -509,6 +617,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } @@ -526,12 +635,14 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + // the first fd used by CAN for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } + if (unsub_groups & (1 << i)) { warnx("unsubscribe from actuator_controls_%d", i); ::close(_control_subs[i]); @@ -583,8 +694,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + } if (_mixers == nullptr) { _groups_required = 0; @@ -600,6 +712,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) _mixers = nullptr; _groups_required = 0; ret = -EINVAL; + } else { _mixers->groups_required(_groups_required); @@ -640,9 +753,10 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); - for (uint8_t i=0; i<_outputs.noutputs; i++) { - printf("%d ", (int)(_outputs.output[i]*1000)); + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i] * 1000)); } + printf("\n"); // ESC status @@ -653,7 +767,8 @@ UavcanNode::print_info() printf("ESC Status:\n"); printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); - for (uint8_t i=0; i<_outputs.noutputs; i++) { + + for (uint8_t i = 0; i < _outputs.noutputs; i++) { printf("%d\t", esc.esc[i].esc_address); printf("%3.2f\t", (double)esc.esc[i].esc_voltage); printf("%3.2f\t", (double)esc.esc[i].esc_current); @@ -669,6 +784,7 @@ UavcanNode::print_info() // Sensor bridges auto br = _sensor_bridges.getHead(); + while (br != nullptr) { printf("Sensor '%s':\n", br->get_name()); br->print_status(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index c6f895c557d3..aa67972b673a 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -47,6 +47,14 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" + +#include +#include +#include +#include +#include + + /** * @file uavcan_main.hpp * @@ -57,18 +65,40 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) - /** * A UAVCAN node. */ class UavcanNode : public device::CDev { + static constexpr unsigned MaxBitRatePerSec = 1000000; + static constexpr unsigned bitPerFrame = 148; + static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; + static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); + + static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - static constexpr unsigned RxQueueLenPerIface = 64; - static constexpr unsigned StackSize = 3000; + + /* + * This memory is reserved for uavcan to use for queuing CAN frames. + * At 1Mbit there is approximately one CAN frame every 145 uS. + * The number of buffers sets how long you can go without calling + * node_spin_xxxx. Since our task is the only one running and the + * driver will light the fd when there is a CAN frame we can nun with + * a minimum number of buffers to conserver memory. Each buffer is + * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate + * of ~1 mS + * 1000000/200 + */ + + static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At + static constexpr unsigned StackSize = 3400; public: typedef uavcan::Node Node; @@ -117,11 +147,19 @@ class UavcanNode : public device::CDev unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer + static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer + Node _node; ///< library instance pthread_mutex_t _node_mutex; UavcanEscController _esc_controller; + + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr;