From 22d46fa73383014172fd0a0df65906683df3be51 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 9 Jul 2015 11:22:35 -1000 Subject: [PATCH 01/29] Merged master_uavcan_modular src/modules/uavcan/ --- src/lib/uavcan | 2 +- src/modules/uavcan/actuators/esc.cpp | 34 +- src/modules/uavcan/actuators/esc.hpp | 8 +- src/modules/uavcan/module.mk | 7 +- src/modules/uavcan/sensors/gnss.cpp | 28 +- src/modules/uavcan/sensors/mag.cpp | 57 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 9 +- src/modules/uavcan/sensors/sensor_bridge.hpp | 21 +- src/modules/uavcan/uavcan_main.cpp | 324 +++++++---- src/modules/uavcan/uavcan_main.hpp | 39 +- src/modules/uavcan/uavcan_servers.cpp | 280 ++++++++++ src/modules/uavcan/uavcan_servers.hpp | 145 +++++ .../uavcan/uavcan_virtual_can_driver.hpp | 506 ++++++++++++++++++ 13 files changed, 1258 insertions(+), 202 deletions(-) create mode 100644 src/modules/uavcan/uavcan_servers.cpp create mode 100644 src/modules/uavcan/uavcan_servers.hpp create mode 100644 src/modules/uavcan/uavcan_virtual_can_driver.hpp diff --git a/src/lib/uavcan b/src/lib/uavcan index 1f1679c75d..4d18f381e5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 +Subproject commit 4d18f381e580dc0e086a8fb69469fa28e88244bc diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index c6719d4817..51ee776a97 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -68,8 +68,8 @@ int UavcanEscController::init() { // ESC status subscription int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("ESC status sub failed %i", res); return res; } @@ -83,9 +83,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || - (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Rate limiting - we don't want to congest the bus */ const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } + _prev_cmd_pub = timestamp; /* @@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - // trim negative values back to 0. Previously - // we set this to 0.1, which meant motors kept - // spinning when armed, but that should be a - // policy decision for a specific vehicle - // type, as it is not appropriate for all - // types of vehicles (eg. fixed wing). + + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). if (scaled < 0.0F) { scaled = 0.0F; - } + } + if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); @@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); + } else { msg.cmd.push_back(static_cast(0)); } @@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm) { if (arm) { _armed_mask = -1; + } else { _armed_mask = 0; } @@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm) { if (arm) { _armed_mask = MOTOR_BIT(num); + } else { _armed_mask = 0; } @@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< } } -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) { _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; if (_esc_status_pub > 0) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c0355422..67bb47cc14 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -54,7 +54,7 @@ class UavcanEscController { public: - UavcanEscController(uavcan::INode& node); + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); int init(); @@ -79,12 +79,12 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; - typedef uavcan::MethodBinder&)> StatusCbBinder; - typedef uavcan::MethodBinder - TimerCbBinder; + typedef uavcan::MethodBinder + TimerCbBinder; bool _armed = false; esc_status_s _esc_status = {}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1d6c205586..da62e55634 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ + uavcan_servers.cpp \ uavcan_clock.cpp \ uavcan_params.c @@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # 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 +override EXTRADEFINES := $(EXTRADEFINES) \ +-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ +-DUAVCAN_NO_ASSERTIONS \ +-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 3ae07367fa..52e4abbaf1 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -46,20 +46,21 @@ const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : -_node(node), -_sub_fix(node), -_report_pub(-1) + _node(node), + _sub_fix(node), + _report_pub(-1) { } int UavcanGnssBridge::init() { int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; } @@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const void UavcanGnssBridge::print_status() const { printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { printf("N/A\n"); + } else { printf("%d\n", _receiver_node_id); } @@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { report.eph = -1.0F; report.epv = -1.0F; } if (valid_velocity_covariance) { - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. @@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure(arg), sizeof(_scale)); - return 0; - } + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { - std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); - return 0; - } + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { - return 0; // Nothing to do - } + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { - return 1; // declare it external rise it's priority and to allow for correct orientation compensation - } + return 1; // declare it external rise it's priority and to allow for correct orientation compensation + } + case MAGIOCSSAMPLERATE: { - return 0; // Pretend that this stuff is supported to keep the sensor app happy - } + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: case MAGIOCGSAMPLERATE: case MAGIOCSRANGE: @@ -121,15 +132,17 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar case MAGIOCSLOWPASS: case MAGIOCEXSTRAP: case MAGIOCGLOWPASS: { - return -EINVAL; - } + return -EINVAL; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure + &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index b370764440..e19235e525 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,7 +45,7 @@ /* * IUavcanSensorBridge */ -void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { list.add(new UavcanBarometerBridge(node)); list.add(new UavcanMagnetometerBridge(node)); @@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } + delete [] _channels; } @@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; log("out of class instances"); @@ -119,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->class_instance = class_instance; channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); + if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -128,6 +131,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } + assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); @@ -136,11 +140,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id >= 0) { out += 1; } } + return out; } @@ -152,6 +158,7 @@ void UavcanCDevSensorBridgeBase::print_status() const if (_channels[i].node_id >= 0) { printf("channel %d: node id %d --> class instance %d\n", i, _channels[i].node_id, _channels[i].class_instance); + } else { printf("channel %d: empty\n", i); } diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index de130b0788..dc2a1983b0 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -45,7 +45,7 @@ /** * A sensor bridge class must implement this interface. */ -class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: static constexpr unsigned MAX_NAME_LEN = 20; @@ -77,7 +77,7 @@ public: * Sensor bridge factory. * Creates all known sensor bridges and puts them in the linked list. */ - static void make_all(uavcan::INode &node, List &list); + static void make_all(uavcan::INode &node, List &list); }; /** @@ -86,8 +86,7 @@ public: */ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev { - struct Channel - { + struct Channel { int node_id = -1; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -104,13 +103,13 @@ protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor, - const unsigned max_channels = DEFAULT_MAX_CHANNELS) : - device::CDev(name, devname), - _max_channels(max_channels), - _class_devname(class_devname), - _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]) + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : + device::CDev(name, devname), + _max_channels(max_channels), + _class_devname(class_devname), + _orb_topic(orb_topic_sensor), + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f4763fce7f..94261ac9cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,15 +53,12 @@ #include #include "uavcan_main.hpp" +#include -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 -#endif + /** * @file uavcan_main.cpp @@ -76,33 +73,28 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; -#if defined(USE_FW_NODE_SERVER) -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; -#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), -#if !defined(USE_FW_NODE_SERVER) _esc_controller(_node) -#else - _esc_controller(_node), - _fileserver_backend(_node), - _node_info_retriever(_node), - _fw_upgrade_trigger(_node, fw_version_checker), - _fw_server(_node, _fileserver_backend) -#endif - { + _task_should_exit = false; + _fw_server_action = None; + _fw_server_status = -1; + _tx_injector = nullptr; _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - const int res = pthread_mutex_init(&_node_mutex, nullptr); + int res = pthread_mutex_init(&_node_mutex, nullptr); + + if (res < 0) { + std::abort(); + } + + res = sem_init(&_server_command_sem, 0 , 0); if (res < 0) { std::abort(); @@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys UavcanNode::~UavcanNode() { + + fw_server(Stop); + if (_task != -1) { + /* tell the task we want it to go away */ _task_should_exit = true; @@ -161,13 +157,127 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - -#if defined(USE_FW_NODE_SERVER) - delete(_server_instance); -#endif + pthread_mutex_destroy(&_node_mutex); + sem_destroy(&_server_command_sem); } +int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) +{ + int rv = -1; + + if (UavcanNode::instance()) { + 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 + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + rv = 0; + } + + return rv; +} + +int UavcanNode::start_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers == nullptr) { + + rv = UavcanServers::start(2, _node); + + if (rv >= 0) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().installRxFrameListener(driver.get()); + * would require a dynamic cast and rtti is not enabled. + */ + UavcanServers::instance()->attachITxQueueInjector(&_tx_injector); + } + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + +int UavcanNode::request_fw_check() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + _servers->requestCheckAllNodesFirmwareAndUpdate(); + rv = 0; + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; + +} + +int UavcanNode::stop_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + _tx_injector = nullptr; + + rv = _servers->stop(); + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + + +int UavcanNode::fw_server(eServerAction action) +{ + int rv = -EAGAIN; + + switch (action) { + case Start: + case Stop: + case CheckFW: + if (_fw_server_action == None) { + _fw_server_action = action; + sem_wait(&_server_command_sem); + rv = _fw_server_status; + } + + break; + + default: + rv = -EINVAL; + break; + } + + return rv; +} + + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; + } + const int node_init_res = _instance->init(node_id); if (node_init_res < 0) { @@ -248,7 +363,7 @@ void UavcanNode::fill_node_info() assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); @@ -256,21 +371,7 @@ void UavcanNode::fill_node_info() /* hardware version */ uavcan::protocol::HardwareVersion hwver; - - 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 - } - - uint8_t udid[12] = {}; // Someone seems to love magic numbers - get_board_serial(udid); - uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); - + getHardwareVersion(hwver); _node.setHardwareVersion(hwver); } @@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } -#if defined(USE_FW_NODE_SERVER) - /* 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; - } - -#endif /* Start the Node */ return _node.start(); @@ -398,6 +431,11 @@ void UavcanNode::node_spin_once() warnx("node spin error %i", spin_res); } + + if (_tx_injector != nullptr) { + _tx_injector->injectTxFramesInto(_node); + } + perf_end(_perfcnt_node_spin_elapsed); } @@ -446,7 +484,7 @@ int UavcanNode::run() * IO multiplexing shall be done here. */ - _node.setStatusOk(); + _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). @@ -466,6 +504,24 @@ int UavcanNode::run() while (!_task_should_exit) { + switch (_fw_server_action) { + case Start: + _fw_server_status = start_fw_server(); + break; + + case Stop: + _fw_server_status = stop_fw_server(); + break; + + case CheckFW: + _fw_server_status = request_fw_check(); + break; + + case None: + default: + break; + } + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { + sem_post(&_server_command_sem); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); @@ -812,7 +870,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop|arm|disarm}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + bool fw = argc > 2 && !std::strcmp(argv[2], "fw"); + if (!std::strcmp(argv[1], "start")) { if (UavcanNode::instance()) { + if (fw && UavcanServers::instance() == nullptr) { + int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); + + if (rv < 0) { + warnx("Firmware Server Failed to Start %d", rv); + ::exit(rv); + } + + ::exit(0); + } + // Already running, no error warnx("already started"); ::exit(0); @@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[]) errx(1, "application not running"); } + if (fw && !std::strcmp(argv[1], "update")) { + if (UavcanServers::instance() == nullptr) { + errx(1, "firmware server is not running"); + } + + int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW); + ::exit(rv); + } + + if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) { + printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped"); + ::exit(0); + } + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); ::exit(0); @@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + if (fw) { + + int rv = inst->fw_server(UavcanNode::Stop); + + if (rv < 0) { + warnx("Firmware Server Failed to Stop %d", rv); + ::exit(rv); + } + + ::exit(0); + + } else { + delete inst; + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 43d82082b4..3413f31bf1 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -48,13 +48,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include -# include -# include -#endif +# include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -66,9 +60,6 @@ #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) @@ -84,8 +75,7 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -99,11 +89,12 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 3400; + static constexpr unsigned StackSize = 1600; public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; + enum eServerAction {None, Start, Stop, CheckFW , Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -126,6 +117,9 @@ public: void print_info(); static UavcanNode *instance() { return _instance; } + static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); + int fw_server(eServerAction action); + void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} private: void fill_node_info(); @@ -133,10 +127,14 @@ private: void node_spin_once(); int run(); int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] - + int start_fw_server(); + int stop_fw_server(); + int request_fw_check(); int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + volatile eServerAction _fw_server_action; + int _fw_server_status; int _armed_sub = -1; ///< uORB subscription of the arming status actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus @@ -151,22 +149,13 @@ private: Node _node; ///< library instance pthread_mutex_t _node_mutex; - + sem_t _server_command_sem; UavcanEscController _esc_controller; - -#if defined(USE_FW_NODE_SERVER) - static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer - - uavcan_posix::BasicFileSeverBackend _fileserver_backend; - uavcan::NodeInfoRetriever _node_info_retriever; - uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; - uavcan::BasicFileServer _fw_server; -#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; - + ITxQueueInjector *_tx_injector; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp new file mode 100644 index 0000000000..6c4d70e3cd --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uavcan_main.hpp" +#include "uavcan_servers.hpp" +#include "uavcan_virtual_can_driver.hpp" + +# include +# include +# include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +# define OK 0 + +/** + * @file uavcan_servers.cpp + * + * Implements basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * David Sidrane + */ + +/* + * UavcanNode + */ +UavcanServers *UavcanServers::_instance; +UavcanServers::UavcanServers(uavcan::INode &main_node) : + _subnode_thread(-1), + _vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _main_node(main_node), + _tracer(), + _storage_backend(), + _fw_version_checker(), + _server_instance(_subnode, _storage_backend, _tracer), + _fileserver_backend(_subnode), + _node_info_retriever(_subnode), + _fw_upgrade_trigger(_subnode, _fw_version_checker), + _fw_server(_subnode, _fileserver_backend), + _mutex_inited(false), + _check_fw(false) + +{ +} + +UavcanServers::~UavcanServers() +{ + if (_mutex_inited) { + (void)Lock::deinit(_subnode_mutex); + } +} + +int UavcanServers::stop(void) +{ + UavcanServers *server = instance(); + + if (server == nullptr) { + warnx("Already stopped"); + return -1; + } + + _instance = nullptr; + + if (server->_subnode_thread != -1) { + pthread_cancel(server->_subnode_thread); + pthread_join(server->_subnode_thread, NULL); + } + + server->_main_node.getDispatcher().removeRxFrameListener(); + + delete server; + return 0; +} + +int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) +{ + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } + + /* + * Node init + */ + _instance = new UavcanServers(main_node); + + if (_instance == nullptr) { + warnx("Out of memory"); + return -2; + } + + int rv = _instance->init(num_ifaces); + + if (rv < 0) { + warnx("Node init failed %i", rv); + delete _instance; + return rv; + } + + /* + * Start the thread. Normally it should never exit. + */ + pthread_attr_t tattr; + struct sched_param param; + + pthread_attr_init(&tattr); + tattr.stacksize = StackSize; + param.sched_priority = Priority; + pthread_attr_setschedparam(&tattr, ¶m); + + static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; + + rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); + + if (rv < 0) { + warnx("start failed: %d", errno); + rv = -errno; + delete _instance; + } + + return rv; +} + +int UavcanServers::init(unsigned num_ifaces) +{ + /* + * Initialize the mutex. + * giving it its path + */ + + int ret = Lock::init(_subnode_mutex); + + if (ret < 0) { + return ret; + } + + _mutex_inited = true; + + _subnode.setNodeID(_main_node.getNodeID()); + _main_node.getDispatcher().installRxFrameListener(&_vdriver); + + + /* + * Initialize the fw version checker. + * giving it its 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; + } + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + UavcanNode::getHardwareVersion(hwver); + + /* Initialize the dynamic node id server */ + ret = _server_instance.init(hwver.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 OK; +} +__attribute__((optimize("-O0"))) +pthread_addr_t UavcanServers::run(pthread_addr_t) +{ + Lock lock(_subnode_mutex); + + while (1) { + + if (_check_fw == true) { + _check_fw = false; + _node_info_retriever.invalidateAll(); + } + + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } + } + + warnx("exiting."); + return (pthread_addr_t) 0; +} + diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp new file mode 100644 index 0000000000..9a68a8cc25 --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 +#include +#include + +#include +#include + +# include +# include +# include +# include +# include +# include +# include +# include + +# include "uavcan_virtual_can_driver.hpp" + +/** + * @file uavcan_servers.hpp + * + * Defines basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * @author David Sidrane + */ + +#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" + +/** + * A UAVCAN Server Sub node. + */ +class UavcanServers +{ + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; + + static constexpr unsigned MaxCanFramsPerTransfer = 63; + + /** + * This number is based on the worst case max number of frames per interface. With + * MemPoolBlockSize set at 48 this is 6048 Bytes. + * + * The servers can be forced to use the primary interface only, this can be achieved simply by passing + * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. + */ + static constexpr unsigned QueuePoolSize = + (UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); + + static constexpr unsigned StackSize = 3500; + static constexpr unsigned Priority = 120; + + typedef uavcan::SubNode SubNode; + +public: + UavcanServers(uavcan::INode &main_node); + + virtual ~UavcanServers(); + + static int start(unsigned num_ifaces, uavcan::INode &main_node); + static int stop(void); + + SubNode &get_node() { return _subnode; } + + static UavcanServers *instance() { return _instance; } + + /* + * Set the main node's pointer to to the injector + * This is a work around as main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;} + + void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; } + +private: + pthread_t _subnode_thread; + pthread_mutex_t _subnode_mutex; + + int init(unsigned num_ifaces); + void deinit(void); + + pthread_addr_t run(pthread_addr_t); + + static UavcanServers *_instance; ///< singleton pointer + + typedef VirtualCanDriver vCanDriver; + + vCanDriver _vdriver; + + uavcan::SubNode _subnode; ///< library instance + uavcan::INode &_main_node; ///< library instance + + uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; + uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; + uavcan_posix::FirmwareVersionChecker _fw_version_checker; + uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + + bool _mutex_inited; + volatile bool _check_fw; + +}; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp new file mode 100644 index 0000000000..7a151d2cd2 --- /dev/null +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Pavel Kirienko + * David Sidrane + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* + * General purpose wrapper around os's mutual exclusion + * mechanism. + * + * It supports the + * + * Note the naming of thier_mutex_ implies that the underlying + * mutex is owned by class using the Lock. + * and this wrapper provides service to initialize and de initialize + * the mutex. + */ +class Lock +{ + pthread_mutex_t &thier_mutex_; + +public: + + Lock(pthread_mutex_t &m) : + thier_mutex_(m) + { + (void)pthread_mutex_lock(&m); + } + + ~Lock() + { + (void)pthread_mutex_unlock(&thier_mutex_); + } + + static int init(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_init(&thier_mutex_, NULL); + } + + static int deinit(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_destroy(&thier_mutex_); + } + +}; + +/** + * Generic queue based on the linked list class defined in libuavcan. + * This class does not use heap memory. + */ +template +class Queue +{ + struct Item : public uavcan::LinkedListNode { + T payload; + + template + Item(Args... args) : payload(args...) { } + }; + + uavcan::LimitedPoolAllocator allocator_; + uavcan::LinkedListRoot list_; + +public: + Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : + allocator_(arg_allocator, block_allocation_quota) + { + uavcan::IsDynamicallyAllocatable::check(); + } + + bool isEmpty() const { return list_.isEmpty(); } + + /** + * Creates one item in-place at the end of the list. + * Returns true if the item was appended successfully, false if there's not enough memory. + * Complexity is O(N) where N is queue length. + */ + template + bool tryEmplace(Args... args) + { + // Allocating memory + void *const ptr = allocator_.allocate(sizeof(Item)); + + if (ptr == nullptr) { + return false; + } + + // Constructing the new item + Item *const item = new(ptr) Item(args...); + assert(item != nullptr); + + // Inserting the new item at the end of the list + Item *p = list_.get(); + + if (p == nullptr) { + list_.insert(item); + + } else { + while (p->getNextListNode() != nullptr) { + p = p->getNextListNode(); + } + + assert(p->getNextListNode() == nullptr); + p->setNextListNode(item); + assert(p->getNextListNode()->getNextListNode() == nullptr); + } + + return true; + } + + /** + * Accesses the first element. + * Nullptr will be returned if the queue is empty. + * Complexity is O(1). + */ + T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; } + const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; } + + /** + * Removes the first element. + * If the queue is empty, nothing will be done and assertion failure will be triggered. + * Complexity is O(1). + */ + void pop() + { + Item *const item = list_.get(); + assert(item != nullptr); + + if (item != nullptr) { + list_.remove(item); + item->~Item(); + allocator_.deallocate(item); + } + } +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + */ +class VirtualCanIface : public uavcan::ICanIface, + uavcan::Noncopyable +{ + /** + * This class re-defines uavcan::RxCanFrame with flags. + * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded, + * rendering this class unusable with Queue<>. + */ + struct RxItem: public uavcan::CanFrame { + const uavcan::MonotonicTime ts_mono; + const uavcan::UtcTime ts_utc; + const uavcan::CanIOFlags flags; + const uint8_t iface_index; + + RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) : + uavcan::CanFrame(arg_frame), + ts_mono(arg_frame.ts_mono), + ts_utc(arg_frame.ts_utc), + flags(arg_flags), + iface_index(arg_frame.iface_index) + { + // Making sure it will fit into a pool block with a pointer prefix + static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee"); + } + }; + + pthread_mutex_t &common_driver_mutex_; + + uavcan::CanTxQueue prioritized_tx_queue_; + Queue rx_queue_; + + int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override + { + Lock lock(common_driver_mutex_); + prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags); + return 1; + } + + int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override + { + Lock lock(common_driver_mutex_); + + if (rx_queue_.isEmpty()) { + return 0; + } + + const auto item = *rx_queue_.peek(); + rx_queue_.pop(); + + out_frame = item; + out_ts_monotonic = item.ts_mono; + out_ts_utc = item.ts_utc; + out_flags = item.flags; + + return 1; + } + + int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; } + uint16_t getNumFilters() const override { return 0; } + uint64_t getErrorCount() const override { return 0; } + +public: + VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock, + pthread_mutex_t &arg_mutex, unsigned quota_per_queue) : + common_driver_mutex_(arg_mutex), + prioritized_tx_queue_(allocator, clock, quota_per_queue), + rx_queue_(allocator, quota_per_queue) + { + } + + ~VirtualCanIface() + { + } + + /** + * Note that RX queue overwrites oldest items when overflowed. + * Call this from the main thread only. + * No additional locking is required. + */ + void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) + { + Lock lock(common_driver_mutex_); + + if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) { + rx_queue_.pop(); + (void)rx_queue_.tryEmplace(frame, flags); + } + } + + /** + * Call this from the main thread only. + * No additional locking is required. + */ + void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index) + { + Lock lock(common_driver_mutex_); + const std::uint8_t iface_mask = static_cast(1U << iface_index); + + while (auto e = prioritized_tx_queue_.peek()) { + UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s", + unsigned(iface_mask), e->toString().c_str()); + + const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask, + uavcan::CanTxQueue::Qos(e->qos), e->flags); + + prioritized_tx_queue_.remove(e); + + if (res <= 0) { + break; + } + + } + } + + /** + * Call this from the sub-node thread only. + * No additional locking is required. + */ + bool hasDataInRxQueue() + { + Lock lock(common_driver_mutex_); + return !rx_queue_.isEmpty(); + } +}; + +/** + * This interface defines one method that will be called by the main node thread periodically in order to + * transfer contents of TX queue of the sub-node into the TX queue of the main node. + */ +class ITxQueueInjector +{ +public: + virtual ~ITxQueueInjector() { } + + /** + * Flush contents of TX queues into the main node. + * @param main_node Reference to the main node. + */ + virtual void injectTxFramesInto(uavcan::INode &main_node) = 0; +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the + * memory pool that will be shared across all interfaces for RX/TX queues. + * Typically this value should be no less than 4K per interface. + */ +template +class VirtualCanDriver : public uavcan::ICanDriver, + public uavcan::IRxFrameListener, + public ITxQueueInjector, + uavcan::Noncopyable +{ + class Event + { + FAR sem_t sem; + + + public: + + int init() + { + return sem_init(&sem, 0, 0); + } + + int deinit() + { + return sem_destroy(&sem); + } + + + Event() + { + } + + ~Event() + { + } + + + /** + */ + + void waitFor(uavcan::MonotonicDuration duration) + { + static const unsigned NsPerSec = 1000000000; + + if (duration.isPositive()) { + auto abstime = ::timespec(); + + if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) { + abstime.tv_nsec += duration.toUSec() * 1000; + + if (abstime.tv_nsec >= NsPerSec) { + abstime.tv_sec++; + abstime.tv_nsec -= NsPerSec; + } + + (void)sem_timedwait(&sem, &abstime); + } + } + } + + void signal() + { + int count; + int rv = sem_getvalue(&sem, &count); + + if (rv > 0 && count <= 0) { + sem_post(&sem); + } + } + }; + + Event event_; ///< Used to unblock the select() call when IO happens. + pthread_mutex_t driver_mutex_; ///< Shared across all ifaces + uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; + const unsigned num_ifaces_; + uavcan::ISystemClock &clock_; + + uavcan::ICanIface *getIface(uint8_t iface_index) override + { + return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr; + } + + uint8_t getNumIfaces() const override { return num_ifaces_; } + + /** + * This and other methods of ICanDriver will be invoked by the sub-node thread. + */ + int16_t select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline) override + { + bool need_block = (inout_masks.write == 0); // Write queue is infinite + + for (unsigned i = 0; need_block && (i < num_ifaces_); i++) { + const bool need_read = inout_masks.read & (1U << i); + + if (need_read && ifaces_[i]->hasDataInRxQueue()) { + need_block = false; + } + } + + if (need_block) { + event_.waitFor(blocking_deadline - clock_.getMonotonic()); + } + + inout_masks = uavcan::CanSelectMasks(); + + for (unsigned i = 0; i < num_ifaces_; i++) { + const std::uint8_t iface_mask = 1U << i; + inout_masks.write |= iface_mask; // Always ready to write + + if (ifaces_[i]->hasDataInRxQueue()) { + inout_masks.read |= iface_mask; + } + } + + return num_ifaces_; // We're always ready to write, hence > 0. + } + + /** + * This handler will be invoked by the main node thread. + */ + void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override + { + UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str()); + + if (frame.iface_index < num_ifaces_) { + ifaces_[frame.iface_index]->addRxFrame(frame, flags); + event_.signal(); + + } + } + + /** + * This method will be invoked by the main node thread. + */ + void injectTxFramesInto(uavcan::INode &main_node) override + { + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i]->flushTxQueueTo(main_node, i); + } + + event_.signal(); + } + +public: + VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + num_ifaces_(arg_num_ifaces), + clock_(system_clock) + { + Lock::init(driver_mutex_); + event_.init(); + + assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); + + const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; + const unsigned quota_per_queue = quota_per_iface; // 2x overcommit + + UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", + unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + } + } + + ~VirtualCanDriver() + { + Lock::deinit(driver_mutex_); + event_.deinit(); + } + +}; From 67f1fbf84418e96a7b21b3825a34d125d6c309ed Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sun, 21 Jun 2015 09:15:59 -1000 Subject: [PATCH 02/29] Need recusive submodule checkout --- Tools/check_submodules.sh | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b251642ce..c2c6e3d7bb 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -70,8 +68,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d src/lib/eigen ] @@ -92,8 +89,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/gencpp ] @@ -114,8 +110,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/genmsg ] @@ -136,8 +131,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi exit 0 From fc6144c46ed952816226ae4d8620efa221b696c6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 22 Jul 2015 09:17:32 -1000 Subject: [PATCH 03/29] Updated Submodule src/lib/uavcan ==master --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 4d18f381e5..4238df3e9f 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 4d18f381e580dc0e086a8fb69469fa28e88244bc +Subproject commit 4238df3e9fe4a01b22d55fb7088319017bdbab8b From 005d0cb0e7d97c3adb5a4f28064b40bded0e61cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 10:04:36 +0200 Subject: [PATCH 04/29] Att pos EKF: Fix init logic, only set local reference if GPS lock present --- .../AttitudePositionEstimatorEKF.h | 2 +- .../ekf_att_pos_estimator_main.cpp | 27 ++++++++++--------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 0dd2b72d92..b1c13ee553 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -335,7 +335,7 @@ private: /** * Initialize the reference position for the local coordinate frame */ - void initReferencePosition(hrt_abstime timestamp, + void initReferencePosition(hrt_abstime timestamp, bool gps_valid, double lat, double lon, float gps_alt, float baro_alt); /** diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 60be85b2cd..07b8c8dfd5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -420,7 +420,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -726,7 +726,7 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (isfinite(_ekf->states[9])) { @@ -738,17 +738,20 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, } // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; _baro_alt_filt = baro_alt; - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = timestamp; + if (gps_valid) { + _gps_alt_filt = gps_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + } } void AttitudePositionEstimatorEKF::initializeGPS() @@ -781,7 +784,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, @@ -1358,7 +1361,7 @@ void AttitudePositionEstimatorEKF::pollData() } //Check if the GPS fix is good enough for us to use - if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + if ((_gps.fix_type >= 3) && (_gps.eph < requiredAccuracy) && (_gps.epv < requiredAccuracy)) { _gpsIsGood = true; } else { From 4c79f8d4a2aac61db8f4c80ae3459cc87c103914 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 8 Aug 2015 16:15:11 +0200 Subject: [PATCH 05/29] Backported VTOL to beta branch. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 14 +++-- ROMFS/px4fmu_common/init.d/rcS | 4 ++ ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 6 +- .../fw_att_control/fw_att_control_main.cpp | 9 +-- src/modules/vtol_att_control/module.mk | 4 +- src/modules/vtol_att_control/tiltrotor.cpp | 58 ++++++++----------- src/modules/vtol_att_control/tiltrotor.h | 3 +- .../vtol_att_control/tiltrotor_params.c | 39 +------------ .../vtol_att_control_main.cpp | 6 ++ .../vtol_att_control/vtol_att_control_main.h | 2 + .../vtol_att_control_params.c | 48 ++++++++++++++- src/modules/vtol_att_control/vtol_type.h | 4 +- 12 files changed, 109 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 1ec65dff3e..52c6258c39 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_P 0.17 param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_FF 0.0 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.8 param set MC_YAW_FF 0.5 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + + param set VT_TILT_MC 0.08 + param set VT_TILT_TRANS 0.5 + param set VT_TILT_FW 0.9 fi set MIXER firefly6 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index dfa270b048..d45cb7c97a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -626,6 +626,10 @@ then then set MAV_TYPE 21 fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi fi # Still no MAV_TYPE found diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 22dc2a69ce..8e31007ff1 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -5,18 +5,18 @@ Tilt mechanism servo mixer --------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 10000 10000 0 -10000 10000 +S: 1 4 0 20000 -10000 -10000 10000 Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 43c44aaeac..9a92f5c70e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -799,6 +798,11 @@ FixedwingAttitudeControl::task_main() //warnx("_actuators_airframe.control[1] = -1.0f;"); } + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { + continue; + } + /* default flaps to center */ float flaps_control = 0.0f; @@ -808,11 +812,8 @@ FixedwingAttitudeControl::task_main() } /* decide if in stabilized or full manual control */ - if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale around tuning airspeed */ - float airspeed; /* if airspeed is not updating, we assume the normal average speed */ diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index ad6efd2b27..d3f9326b04 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \ tiltrotor_params.c \ tiltrotor.cpp \ vtol_type.cpp \ - tailsitter.cpp + tailsitter.cpp \ + standard_params.c \ + standard.cpp EXTRACXXFLAGS = -Wno-write-strings diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2cf074fe42..e7ebab58e9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state() void Tiltrotor::process_mc_data() { - fill_mc_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_fw_state() @@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data() void Tiltrotor::process_fw_data() { - fill_fw_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_transition_state() { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition) { - _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + if (_params_tiltrotor.front_trans_dur <= 0.0f) { + _tilt_control = _params_tiltrotor.tilt_transition; + } else if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) { _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); } else { // at low speeds give full weight to mc @@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state() _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f); _roll_weight_mc = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { // tilt rotors forward up to certain angle - float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress; } _roll_weight_mc = progress; @@ -263,38 +267,26 @@ void Tiltrotor::update_external_state() } /** -* Prepare message to acutators with data from mc attitude controller. +* Prepare message to acutators with data from the attitude controllers. */ -void Tiltrotor::fill_mc_att_control_output() +void Tiltrotor::fill_att_control_output() { - _actuators_out_0->control[0] = _actuators_mc_in->control[0]; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2]; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle + } else { + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle + } _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon - + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control -} -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void Tiltrotor::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - /*controls for the elevons */ - _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon - _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle - _actuators_out_1->control[4] = _tilt_control; + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw } /** diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 3ef1362d00..07f3562027 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -99,8 +99,7 @@ private: float _tilt_control; float _roll_weight_mc; - void fill_mc_att_control_output(); - void fill_fw_att_control_output(); + void fill_att_control_output(); void set_max_mc(); void set_max_fw(unsigned pwm_value); diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 7d233f6f50..d4e925f072 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -39,28 +39,6 @@ */ #include - -/** - * Duration of a front transition - * - * Time in seconds used for a transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); - -/** - * Duration of a back transition - * - * Time in seconds used for a back transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); /** * Position of tilt servo in mc mode @@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); /** * Position of tilt servo in transition mode @@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); /** * Position of tilt servo in fw mode @@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); - -/** - * Transition airspeed - * - * Airspeed at which we can switch to fw mode - * - * @min 0.0 - * @max 20 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index b70cd19dd8..0d462423b6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() : } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { + _standard = new Standard(this); + _vtol_type = _standard; } else { _task_should_exit = true; } @@ -530,6 +533,9 @@ void VtolAttitudeControl::task_main() // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); + + _vtol_type->process_mc_data(); + fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 43e8969929..bf9a69d5c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include "tiltrotor.h" #include "tailsitter.h" +#include "standard.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); @@ -188,6 +189,7 @@ private: VtolType * _vtol_type; // base class for different vtol types Tiltrotor * _tiltrotor; // tailsitter vtol type Tailsitter * _tailsitter; // tiltrotor vtol type + Standard * _standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 33c095036c..56779ca8f9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** - * VTOL Type (Tailsitter=0, Tiltrotor=1) + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * * @min 0 - * @max 1 + * @max 2 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); @@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 1.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index bbe6a8642e..a7fdbdde1a 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -38,8 +38,8 @@ * */ -#ifndef VTOL_YYPE_H -#define VTOL_YYPE_H +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H struct Params { int idle_pwm_mc; // pwm value for idle in mc mode From 9ef34b5d1cb2dbf6f5f61ed98031020274b3af42 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 8 Aug 2015 16:16:59 +0200 Subject: [PATCH 06/29] Add standard VTOL airframe. --- .../init.d/13005_vtol_AAERT_quad | 50 +++ .../px4fmu_common/init.d/13005_vtol_AERT_quad | 50 +++ ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix | 32 ++ ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix | 30 ++ .../px4fmu_common/mixers/vtol_quad_x.main.mix | 4 + src/modules/vtol_att_control/standard.cpp | 296 ++++++++++++++++++ src/modules/vtol_att_control/standard.h | 107 +++++++ .../vtol_att_control/standard_params.c | 51 +++ 8 files changed, 620 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix create mode 100644 src/modules/vtol_att_control/standard.cpp create mode 100644 src/modules/vtol_att_control/standard.h create mode 100644 src/modules/vtol_att_control/standard_params.c diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad new file mode 100644 index 0000000000..4b8191f557 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad @@ -0,0 +1,50 @@ +#!nsh +# +# @name Generic AAERT tailplane airframe with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Simon Wilks +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad new file mode 100644 index 0000000000..a3e183a84a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad @@ -0,0 +1,50 @@ +#!nsh +# +# @name Generic AERT with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Simon Wilks +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix new file mode 100644 index 0000000000..f3e9c42354 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix @@ -0,0 +1,32 @@ +Mixer for an AAERT VTOL +======================= + +Aileron 1 mixer +--------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Aileron 2 mixer +--------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Elevator mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 + +Throttle mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix new file mode 100644 index 0000000000..260e60840a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix @@ -0,0 +1,30 @@ +Mixer for a horizontal format with X-Quad and tractor AERT plane configuration +============================================================================== + +Motor speed mixer +----------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + +Aileron mixers +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Elevator mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix new file mode 100644 index 0000000000..92bfce6c8c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix @@ -0,0 +1,4 @@ +# VTOL quad X mixer for PX4FMU +#============================= + +R: 4x 10000 10000 10000 0 diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp new file mode 100644 index 0000000000..c38a935ff7 --- /dev/null +++ b/src/modules/vtol_att_control/standard.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file standard.cpp + * + * @author Simon Wilks + * @author Roman Bapst + * +*/ + +#include "standard.h" +#include "vtol_att_control_main.h" + +Standard::Standard(VtolAttitudeControl *attc) : + VtolType(attc), + _flag_enable_mc_motors(true), + _pusher_throttle(0.0f), + _mc_att_ctl_weight(1.0f), + _airspeed_trans_blend_margin(0.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); + _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); + } + +Standard::~Standard() +{ +} + +int +Standard::parameters_update() +{ + float v; + + /* duration of a forwards transition to fw mode */ + param_get(_params_handles_standard.front_trans_dur, &v); + _params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* duration of a back transition to mc mode */ + param_get(_params_handles_standard.back_trans_dur, &v); + _params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* target throttle value for pusher motor during the transition to fw mode */ + param_get(_params_handles_standard.pusher_trans, &v); + _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); + + /* airspeed at which it we should switch to fw mode */ + param_get(_params_handles_standard.airspeed_trans, &v); + _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); + + /* airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_standard.airspeed_blend, &v); + _params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f); + + _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; + + return OK; +} + +void Standard::update_vtol_state() +{ + parameters_update(); + + /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * For the back transition the pusher motor is immediately stopped and rotors reactivated. + */ + + if (_manual_control_sp->aux1 < 0.0f) { + // the transition to fw mode switch is off + if (_vtol_schedule.flight_mode == MC_MODE) { + // in mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // transition to mc mode + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // failsafe back to mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // keep transitioning to mc mode + _vtol_schedule.flight_mode = MC_MODE; + } + + // the pusher motor should never be powered when in or transitioning to mc mode + _pusher_throttle = 0.0f; + + } else { + // the transition to fw mode switch is on + if (_vtol_schedule.flight_mode == MC_MODE) { + // start transition to fw mode + _vtol_schedule.flight_mode = TRANSITION_TO_FW; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // in fw mode + _vtol_schedule.flight_mode = FW_MODE; + _mc_att_ctl_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + _vtol_schedule.flight_mode = FW_MODE; + // we can turn off the multirotor motors now + _flag_enable_mc_motors = false; + // don't set pusher throttle here as it's being ramped up elsewhere + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // transitioning to mc mode & transition switch on - failsafe back into fw mode + _vtol_schedule.flight_mode = FW_MODE; + } + } + + // map specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; + } +} + +void Standard::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_params_standard.front_trans_dur <= 0.0f) { + // just set the final target throttle value + _pusher_throttle = _params_standard.pusher_trans; + } else if (_pusher_throttle <= _params_standard.pusher_trans) { + // ramp up throttle to the target throttle value + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + } + + // do blending of mc and fw controls if a blending airspeed has been provided + if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { + _mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + } else { + // at low speeds give full weight to mc + _mc_att_ctl_weight = 1.0f; + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // continually increase mc attitude control as we transition back to mc mode + if (_params_standard.back_trans_dur > 0.0f) { + _mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + } else { + _mc_att_ctl_weight = 1.0f; + } + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (_flag_enable_mc_motors) { + set_max_mc(2000); + set_idle_mc(); + _flag_enable_mc_motors = false; + } + } + + _mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f); +} + +void Standard::update_mc_state() +{ + // do nothing +} + +void Standard::process_mc_data() +{ + fill_att_control_output(); +} + +void Standard::process_fw_data() +{ + fill_att_control_output(); +} + + void Standard::update_fw_state() +{ + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (!_flag_enable_mc_motors) { + set_max_mc(950); + set_idle_fw(); // force them to stop, not just idle + _flag_enable_mc_motors = true; + } + } + +void Standard::update_external_state() +{ +} + +/** + * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine + * what proportion of control should be applied to each of the control groups (mc and fw). + */ +void Standard::fill_att_control_output() +{ + /* multirotor controls */ + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle + + /* fixed wing controls */ + const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight; + _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch + _actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw + + // set the fixed wing throttle control + if (_vtol_schedule.flight_mode == FW_MODE) { + // take the throttle value commanded by the fw controller + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; + } else { + // otherwise we may be ramping up the throttle during the transition to fw mode + _actuators_out_1->control[3] = _pusher_throttle; + } +} + +/** +* Disable all multirotor motors when in fw mode. +*/ +void +Standard::set_max_mc(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h new file mode 100644 index 0000000000..e4e1d92d85 --- /dev/null +++ b/src/modules/vtol_att_control/standard.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + + /** + * @file standard.h + * VTOL with fixed multirotor motor configurations (such as quad) and a pusher + * (or puller aka tractor) motor for forward flight. + * + * @author Simon Wilks + * @author Roman Bapst + * + */ + +#ifndef STANDARD_H +#define STANDARD_H +#include "vtol_type.h" +#include +#include + +class Standard : public VtolType +{ + +public: + + Standard(VtolAttitudeControl * _att_controller); + ~Standard(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float pusher_trans; + float airspeed_blend; + float airspeed_trans; + } _params_standard; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t pusher_trans; + param_t airspeed_blend; + param_t airspeed_trans; + } _params_handles_standard; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_TO_FW, + TRANSITION_TO_MC, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + }_vtol_schedule; + + bool _flag_enable_mc_motors; + float _pusher_throttle; + float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning + float _airspeed_trans_blend_margin; + + void fill_att_control_output(); + void set_max_mc(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c new file mode 100644 index 0000000000..134dcd0b8a --- /dev/null +++ b/src/modules/vtol_att_control/standard_params.c @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file standard_params.c + * Parameters for the standard VTOL controller. + * + * @author Simon Wilks + * @author Roman Bapst + */ + +#include + +/** + * Target throttle value for pusher/puller motor during the transition to fw mode + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f); From 6b6fe5d22977bb035833b88d6c962d7b1b20e0a9 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 9 Aug 2015 11:36:13 +0200 Subject: [PATCH 07/29] Remove old config and mixer file that slipped into the port. --- .../px4fmu_common/init.d/13005_vtol_AERT_quad | 50 ------------------- ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix | 30 ----------- 2 files changed, 80 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad delete mode 100644 ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad deleted file mode 100644 index a3e183a84a..0000000000 --- a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad +++ /dev/null @@ -1,50 +0,0 @@ -#!nsh -# -# @name Generic AERT with Quad VTOL. -# -# @type Standard VTOL -# -# @maintainer Simon Wilks -# - -sh /etc/init.d/rc.vtol_defaults - -if [ $AUTOCNF == yes ] -then - param set VT_TYPE 2 - param set VT_MOT_COUNT 4 - param set VT_TRANS_THR 0.75 - - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 - param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 - param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 - param set MC_YAW_FF 0.5 - param set MC_YAWRATE_P 0.22 - param set MC_YAWRATE_I 0.02 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_FF 0.0 -fi - -set MIXER vtol_quad_x -set PWM_OUT 12345678 - -set MIXER_AUX vtol_AERT -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 - -set MAV_TYPE 22 - -param set VT_MOT_COUNT 4 -param set VT_IDLE_PWM_MC 1080 -param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix deleted file mode 100644 index 260e60840a..0000000000 --- a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix +++ /dev/null @@ -1,30 +0,0 @@ -Mixer for a horizontal format with X-Quad and tractor AERT plane configuration -============================================================================== - -Motor speed mixer ------------------ -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 3 0 20000 -10000 -10000 10000 - -Aileron mixers --------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 - -Elevator mixer --------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 - -Rudder mixer ------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 From db975a10c2c41330eaa9cf2ba49d1e2625fd46e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Aug 2015 12:29:42 +0200 Subject: [PATCH 08/29] Fix coning handling in att_pos EKF. Fixes #2663 --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 8 ++++---- src/modules/ekf_att_pos_estimator/estimator_22states.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 967402fff7..3177ab75ef 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -96,6 +96,7 @@ AttPosEKF::AttPosEKF() : correctedDelVel(), summedDelAng(), summedDelVel(), + prevDelAng(), accNavMag(), earthRateNED(), angRate(), @@ -273,12 +274,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; - // Save current measurements - Vector3f prevDelAng = correctedDelAng; - // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + prevDelAng = correctedDelAng; // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); @@ -3290,6 +3289,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + prevDelAng.zero(); dAngIMU.zero(); dVelIMU.zero(); lastGyroOffset.zero(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 9b23f4df44..c9a217a2bb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -139,6 +139,7 @@ public: Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f prevDelAng; ///< previous delta angle, used for coning correction float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) From c287b03d1dd50e5ad84e28b5b87ee65bfc40bece Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Aug 2015 23:27:16 +0200 Subject: [PATCH 09/29] Commander: Do not spam console when not connected --- src/modules/commander/commander.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a01b33087f..b36cb7480b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1361,7 +1361,9 @@ int commander_thread_main(int argc, char *argv[]) /* we first connect a link or re-connect a link after loosing it */ (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) && /* and this link has a communication partner */ - telemetry.heartbeat_time > 0 && + (telemetry.heartbeat_time > 0) && + /* and it is still connected */ + (hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) && /* and the system is not already armed (and potentially flying) */ !armed.armed) { From c5ec4de6eab8aa7639f263aa42c88f0d76061820 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Aug 2015 23:26:35 +0200 Subject: [PATCH 10/29] Increase NSH back-off time --- src/systemcmds/nshterm/nshterm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bdfd26a64d..320d338a23 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -68,8 +68,8 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; - /* back off 1500 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 1500U * 1000U) { + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { usleep(50000); } From 9fa87d1444c5fb8f258914eb917aff660dfcffc2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Aug 2015 04:45:32 +0300 Subject: [PATCH 11/29] LIbuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 4238df3e9f..c2fec7be38 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 4238df3e9fe4a01b22d55fb7088319017bdbab8b +Subproject commit c2fec7be38bcf8fdfb1369f28006c77f7d509d82 From e278419e849d56e24e79e7601072879a0be594d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Aug 2015 05:12:46 +0300 Subject: [PATCH 12/29] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c2fec7be38..46793c5e06 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c2fec7be38bcf8fdfb1369f28006c77f7d509d82 +Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 From aa412aacede3f8ced508600fcf94d8d8247b2fce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Aug 2015 09:42:16 +0300 Subject: [PATCH 13/29] UAVCAN servers: proper handling of startup failure, more verbose error reporting --- src/modules/uavcan/uavcan_servers.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 6c4d70e3cd..d6d72a2f53 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -142,8 +142,9 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) int rv = _instance->init(num_ifaces); if (rv < 0) { - warnx("Node init failed %i", rv); + warnx("Node init failed: %d", rv); delete _instance; + _instance = nullptr; return rv; } @@ -163,9 +164,10 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); if (rv < 0) { - warnx("start failed: %d", errno); + warnx("pthread_create() failed: %d", errno); rv = -errno; delete _instance; + _instance = nullptr; } return rv; @@ -173,6 +175,8 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) int UavcanServers::init(unsigned num_ifaces) { + errno = 0; + /* * Initialize the mutex. * giving it its path @@ -181,6 +185,7 @@ int UavcanServers::init(unsigned num_ifaces) int ret = Lock::init(_subnode_mutex); if (ret < 0) { + warnx("Lock init: %d", errno); return ret; } @@ -197,6 +202,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); if (ret < 0) { + warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno); return ret; } @@ -205,6 +211,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_server.start(); if (ret < 0) { + warnx("BasicFileServer init: %d, errno: %d", ret, errno); return ret; } @@ -213,6 +220,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _storage_backend.init(UAVCAN_NODE_DB_PATH); if (ret < 0) { + warnx("FileStorageBackend init: %d, errno: %d", ret, errno); return ret; } @@ -221,6 +229,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _tracer.init(UAVCAN_LOG_FILE); if (ret < 0) { + warnx("FileEventTracer init: %d, errno: %d", ret, errno); return ret; } @@ -232,6 +241,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _server_instance.init(hwver.unique_id); if (ret < 0) { + warnx("CentralizedServer init: %d", ret); return ret; } @@ -240,6 +250,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _node_info_retriever.start(); if (ret < 0) { + warnx("NodeInfoRetriever init: %d", ret); return ret; } @@ -248,6 +259,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); if (ret < 0) { + warnx("FirmwareUpdateTrigger init: %d", ret); return ret; } From 36f91d30ebf6d617e4d3dc3299855a59598b4170 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Aug 2015 09:53:13 +0300 Subject: [PATCH 14/29] Fixes pure virtual call exception in VirtualCanDriver<>::handleRxFrame() Stack trace: Thread [1] (Suspended: Breakpoint hit.) 12 __cxa_pure_virtual() libxx_cxapurevirtual.cxx:66 0x0808ca42 11 notifyRxFrameListener() uc_dispatcher.cpp:216 0x080787ce 10 uavcan::Dispatcher::spinOnce() uc_dispatcher.cpp:276 0x080787ce 9 uavcan::Scheduler::spinOnce() uc_scheduler.cpp:196 0x0807b4e0 8 spinOnce() abstract_node.hpp:88 0x080659fc 7 spinOnce() node.hpp:132 0x080659fc 6 UavcanNode::node_spin_once() uavcan_main.cpp:428 0x080659fc 5 UavcanNode::run() uavcan_main.cpp:542 0x08065e74 4 operator() uavcan_main.cpp:343 0x0806626a 3 UavcanNode::__lambda0::_FUN() uavcan_main.cpp:343 0x0806626a 2 task_start() task_start.c:138 0x08087720 1 0x00000000 --- src/modules/uavcan/uavcan_servers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index d6d72a2f53..1713b282e7 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -98,6 +98,7 @@ UavcanServers::~UavcanServers() if (_mutex_inited) { (void)Lock::deinit(_subnode_mutex); } + _main_node.getDispatcher().removeRxFrameListener(); } int UavcanServers::stop(void) From 8ec5b0321150855d97ad5c6284865d661c7efa9a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 16:29:54 +0200 Subject: [PATCH 15/29] Disable VTOL airframes since they are not fully stable in beta --- .../{13001_caipirinha_vtol => INACTIVE_13001_caipirinha_vtol} | 0 .../init.d/{13002_firefly6 => INACTIVE_13002_firefly6} | 0 .../{13003_quad_tailsitter => INACTIVE_13003_quad_tailsitter} | 0 .../{13004_quad+_tailsitter => INACTIVE_13004_quad+_tailsitter} | 0 .../{13005_vtol_AAERT_quad => INACTIVE_13005_vtol_AAERT_quad} | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/init.d/{13001_caipirinha_vtol => INACTIVE_13001_caipirinha_vtol} (100%) rename ROMFS/px4fmu_common/init.d/{13002_firefly6 => INACTIVE_13002_firefly6} (100%) rename ROMFS/px4fmu_common/init.d/{13003_quad_tailsitter => INACTIVE_13003_quad_tailsitter} (100%) rename ROMFS/px4fmu_common/init.d/{13004_quad+_tailsitter => INACTIVE_13004_quad+_tailsitter} (100%) rename ROMFS/px4fmu_common/init.d/{13005_vtol_AAERT_quad => INACTIVE_13005_vtol_AAERT_quad} (100%) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol similarity index 100% rename from ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol rename to ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 similarity index 100% rename from ROMFS/px4fmu_common/init.d/13002_firefly6 rename to ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/13003_quad_tailsitter rename to ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter rename to ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad similarity index 100% rename from ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad rename to ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad From 7c33e1fdb5c89b192586f7d0ba406d07431d9c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:51:28 +0200 Subject: [PATCH 16/29] Update NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 678eea3d2c..8891d035df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4 +Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee From a38d954d9afc08984b747af45fab1684c5e57650 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:52:05 +0200 Subject: [PATCH 17/29] Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index d23a551e10..c390a63abf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit d23a551e108d92c7a49a66d162cb9d542bbe6be1 +Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118 From 4ec16efe686afbad267f58e7b859fa6638f46afb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 22:58:09 +0300 Subject: [PATCH 18/29] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 46793c5e06..181b4f6094 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 +Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 From 473d321af8bead4bd45299ad723b4c7beec94bce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:04:41 +0300 Subject: [PATCH 19/29] UAVCAN Magnetometer driver update - no logical changes --- src/modules/uavcan/sensors/mag.cpp | 10 +++++----- src/modules/uavcan/sensors/mag.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index d08f5ed32b..ef9613b1f7 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -45,7 +45,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why? _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; @@ -141,16 +141,16 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; unlock(); publish(msg.getSrcNodeID().get(), &_report); diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index b74afeb0be..a13883a494 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -40,7 +40,7 @@ #include "sensor_bridge.hpp" #include -#include +#include class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { @@ -57,14 +57,14 @@ private: ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, void (UavcanMagnetometerBridge::*) - (const uavcan::ReceivedDataStructure &) > + (const uavcan::ReceivedDataStructure &) > MagCbBinder; - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; mag_report _report = {}; }; From 3860dbdc8cfec81971f709b696b6d1f3a7a8ca8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:07:16 +0300 Subject: [PATCH 20/29] UAVCAN Barometer driver fix - reporting temperature in Celsius degrees, not in Kelvin --- src/modules/uavcan/sensors/baro.cpp | 8 +++----- src/modules/uavcan/sensors/baro.hpp | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8198d1d994..50655d9180 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -46,9 +46,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : _sub_air_pressure_data(node), _sub_air_temperature_data(node), _reports(nullptr) -{ - last_temperature = 0.0f; -} +{ } int UavcanBarometerBridge::init() { @@ -154,7 +152,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { - last_temperature = msg.static_temperature; + last_temperature_kelvin = msg.static_temperature; } void UavcanBarometerBridge::air_pressure_sub_cb(const @@ -163,7 +161,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = last_temperature; + report.temperature = last_temperature_kelvin - 273.15F; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 98dc5cf42f..4024cb156b 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -77,6 +77,6 @@ private: uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; RingBuffer *_reports; - float last_temperature; + float last_temperature_kelvin = 0.0; }; From 9b32a55b402d9c1d9a6bce716c1a03f2b2c1e1cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Aug 2015 14:58:26 +0300 Subject: [PATCH 21/29] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 181b4f6094..bfee82f3c4 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 +Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 From 4296dac884220d2a255ff02b5c1a87cd0a19c4a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:06:26 +0300 Subject: [PATCH 22/29] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index bfee82f3c4..b317b94d98 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 +Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 From 6864779fa6bc3e895461d47e1322f4e90c642d7a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:51:36 +0300 Subject: [PATCH 23/29] Multiple stages for UAVCAN_ENABLE --- ROMFS/px4fmu_common/init.d/rc.uavcan | 30 ++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_params.c | 7 +++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 08ba86d789..9f38ff90bc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,7 +3,26 @@ # UAVCAN initialization script. # +# +# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable. +# TODO there should be a smarter way. +# +set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 +then + set UAVCAN_ENABLE 1 +fi +if param compare UAVCAN_ENABLE 2 +then + set UAVCAN_ENABLE 2 +fi + +echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" + +# +# Starting stuff according to UAVCAN_ENABLE value +# +if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then @@ -16,3 +35,14 @@ then tone_alarm $TUNE_ERR fi fi + +if [ $UAVCAN_ENABLE -ge 2 ] +then + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi +fi diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index e6ea8a8fb7..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -41,10 +41,13 @@ /** * Enable UAVCAN. * - * Enables support for UAVCAN-interfaced actuators and sensors. + * Allowed values: + * 0 - UAVCAN disabled. + * 1 - Enabled support for UAVCAN actuators and sensors. + * 2 - Enabled support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 1 + * @max 2 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); From e1f33871efd87f4b910390c853a9d301aaab8028 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 17:25:23 +0300 Subject: [PATCH 24/29] UAVCAN enabled by default --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 934263b6b4..76d0e8ea09 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); /** * UAVCAN Node ID. From 0e10638c7e0d43c6ed4d608ee418e759b0b77202 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 23:11:12 +0300 Subject: [PATCH 25/29] Temporary fix to UAVCAN sensor prioritization issue, see #2081, #2715 --- ROMFS/px4fmu_common/init.d/rc.uavcan | 25 +++++++++++++------------ ROMFS/px4fmu_common/init.d/rcS | 9 +++++---- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 9f38ff90bc..3615c177de 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -10,11 +10,11 @@ set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 then - set UAVCAN_ENABLE 1 + set UAVCAN_ENABLE 1 fi if param compare UAVCAN_ENABLE 2 then - set UAVCAN_ENABLE 2 + set UAVCAN_ENABLE 2 fi echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" @@ -26,9 +26,6 @@ if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs - sleep 1 echo "[i] UAVCAN started" else echo "[i] ERROR: Could not start UAVCAN" @@ -38,11 +35,15 @@ fi if [ $UAVCAN_ENABLE -ge 2 ] then - if uavcan start fw - then - echo "[i] UAVCAN servers started" - else - echo "[i] ERROR: Could not start UAVCAN servers" - tone_alarm $TUNE_ERR - fi + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi fi + +# First sensor publisher to initialize takes lowest instance ID +# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs +sleep 8 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c1c8dc303d..b21ca9ec88 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -299,6 +299,11 @@ then then fi + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors System (start before Commander so Preflight checks are properly run) # @@ -491,10 +496,6 @@ then fi fi - # UAVCAN - # - sh /etc/init.d/rc.uavcan - # # Logging # From 281cf49586f28434880ba422e03ef3b3e7567c3e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 05:45:00 +0300 Subject: [PATCH 26/29] Libuavcan update - fixed file IO loops --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index b317b94d98..cf39ecf879 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 +Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e From 59fd94da3e75dc8a827979a5f19ca93f88efa4a7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 12:37:51 +0300 Subject: [PATCH 27/29] Libuavcan synced with the version that has DSDL v1.0.0rc1 - no logical changes --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index cf39ecf879..3ae5400aa5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e +Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd From 08fde4f505a8ae085fb49572add2028331dc8998 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 20:19:02 +0300 Subject: [PATCH 28/29] UAVCAN startup delay fix --- ROMFS/px4fmu_common/init.d/rc.uavcan | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 3615c177de..37e6893bd1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -44,6 +44,9 @@ then fi fi -# First sensor publisher to initialize takes lowest instance ID -# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs -sleep 8 +if [ $UAVCAN_ENABLE -ge 1 ] +then + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs + sleep 8 +fi From f371ece43b5e6b028a4be4c90c06136adf087e22 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 22:56:49 +0300 Subject: [PATCH 29/29] UAVCAN_ENABLE defaults to 0 --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 76d0e8ea09..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); /** * UAVCAN Node ID.