diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 64f03133e3..9f8a374aa7 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -225,7 +225,6 @@ class Graph(object): ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), - ('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'), ('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'), diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index a35705f726..a252e37f64 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -64,6 +64,8 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10}; static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors +static constexpr wq_config_t uavcan{"uavcan", 2400, -13}; + static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12}; static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index e6a4dc5b1b..75d76f4ef4 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -129,6 +129,8 @@ px4_add_module( px4_uavcan_dsdlc mixer + mixer_module + output_limit version git_uavcan diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index ac67f00fc0..b5d35d394f 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -56,8 +56,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : UavcanEscController::~UavcanEscController() { - perf_free(_perfcnt_invalid_input); - perf_free(_perfcnt_scaling_error); } int @@ -79,14 +77,14 @@ UavcanEscController::init() } void -UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { - if ((outputs == nullptr) || - (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { + if (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) { + num_outputs = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + } - perf_count(_perfcnt_invalid_input); - return; + if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) { + num_outputs = esc_status_s::CONNECTED_ESC_MAX; } /* @@ -106,34 +104,12 @@ UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) */ uavcan::equipment::esc::RawCommand msg; - actuator_outputs_s actuator_outputs{}; - actuator_outputs.noutputs = num_outputs; - - static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F; - 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 minimum - if (scaled < cmd_min) { - scaled = cmd_min; - perf_count(_perfcnt_scaling_error); - } - - if (scaled > cmd_max) { - scaled = cmd_max; - perf_count(_perfcnt_scaling_error); - } - - msg.cmd.push_back(static_cast(scaled)); - - actuator_outputs.output[i] = scaled; + if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) { + msg.cmd.push_back(static_cast(0)); } else { - msg.cmd.push_back(static_cast(0)); - actuator_outputs.output[i] = 0.0f; + msg.cmd.push_back(static_cast(outputs[i])); } } @@ -161,32 +137,6 @@ UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Note that for a quadrotor it takes one CAN frame */ _uavcan_pub_raw_cmd.broadcast(msg); - - // Publish actuator outputs - actuator_outputs.timestamp = hrt_absolute_time(); - _actuator_outputs_pub.publish(actuator_outputs); -} - -void -UavcanEscController::arm_all_escs(bool arm) -{ - if (arm) { - _armed_mask = -1; - - } else { - _armed_mask = 0; - } -} - -void -UavcanEscController::arm_single_esc(int num, bool arm) -{ - if (arm) { - _armed_mask = MOTOR_BIT(num); - - } else { - _armed_mask = 0; - } } void @@ -212,7 +162,7 @@ UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) _esc_status.esc_count = _rotor_count; _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; - _esc_status.esc_online_flags = UavcanEscController::check_escs_status(); + _esc_status.esc_online_flags = check_escs_status(); _esc_status_pub.publish(_esc_status); } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 471524f5d5..f59fb20b99 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -52,28 +52,28 @@ #include #include #include +#include class UavcanEscController { public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable + static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX; + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); int init(); - void update_outputs(float *outputs, unsigned num_outputs); - - void arm_all_escs(bool arm); - void arm_single_esc(int num, bool arm); - - void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; } + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); /** * Sets the number of rotors */ void set_rotor_count(uint8_t count) { _rotor_count = count; } - static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable + static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); } private: /** @@ -100,15 +100,11 @@ private: typedef uavcan::MethodBinder TimerCbBinder; - bool _armed{false}; - bool _run_at_idle_throttle_when_armed{false}; - esc_status_s _esc_status{}; - uORB::PublicationMulti _actuator_outputs_pub{ORB_ID(actuator_outputs)}; uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; - uint8_t _rotor_count = 0; + uint8_t _rotor_count{0}; /* * libuavcan related things @@ -122,12 +118,5 @@ private: /* * ESC states */ - uint32_t _armed_mask{0}; uint8_t _max_number_of_nonzero_outputs{0}; - - /* - * Perf counters - */ - perf_counter_t _perfcnt_invalid_input{perf_alloc(PC_COUNT, "uavcan_esc_invalid_input")}; - perf_counter_t _perfcnt_scaling_error{perf_alloc(PC_COUNT, "uavcan_esc_scaling_error")}; }; diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp index 6f2b701e85..9fd49917d2 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp @@ -318,6 +318,8 @@ public: * This is designed for use with iface activity LEDs. */ bool hadActivity(); + + BusEvent& updateEvent() { return update_event_; } }; /** diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp index 726721312b..9515f93cca 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp @@ -28,30 +28,17 @@ class CanDriver; */ class BusEvent : uavcan::Noncopyable { - static const unsigned MaxPollWaiters = 8; - - ::file_operations file_ops_; - ::pollfd* pollset_[MaxPollWaiters]; - CanDriver& can_driver_; - bool signal_; - - static int openTrampoline(::file* filp); - static int closeTrampoline(::file* filp); - static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup); - - int open(::file* filp); - int close(::file* filp); - int poll(::file* filp, ::pollfd* fds, bool setup); - - int addPollWaiter(::pollfd* fds); - int removePollWaiter(::pollfd* fds); + using SignalCallbackHandler = void(*)(); + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; public: - static const char* const DevName; BusEvent(CanDriver& can_driver); ~BusEvent(); + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + bool wait(uavcan::MonotonicDuration duration); void signalFromInterrupt(); diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp index 4c7db5402b..071044b5f3 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp @@ -11,148 +11,49 @@ namespace uavcan_kinetis { - -const unsigned BusEvent::MaxPollWaiters; -const char* const BusEvent::DevName = "/dev/uavcan/busevent"; - -int BusEvent::openTrampoline(::file* filp) -{ - return static_cast(filp->f_inode->i_private)->open(filp); -} - -int BusEvent::closeTrampoline(::file* filp) -{ - return static_cast(filp->f_inode->i_private)->close(filp); -} - -int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) -{ - return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); -} - -int BusEvent::open(::file* filp) -{ - (void)filp; - return 0; -} - -int BusEvent::close(::file* filp) -{ - (void)filp; - return 0; -} - -int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) -{ - CriticalSectionLocker locker; - int ret = -1; - - if (setup) - { - ret = addPollWaiter(fds); - if (ret == 0) - { - /* - * Two events can be reported via POLLIN: - * - The RX queue is not empty. This event is level-triggered. - * - Transmission complete. This event is edge-triggered. - * FIXME Since TX event is edge-triggered, it can be lost between poll() calls. - */ - fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0); - if (fds->revents != 0) - { - (void)sem_post(fds->sem); - } - } - } - else - { - ret = removePollWaiter(fds); - } - - return ret; -} - -int BusEvent::addPollWaiter(::pollfd* fds) -{ - for (unsigned i = 0; i < MaxPollWaiters; i++) - { - if (pollset_[i] == UAVCAN_NULLPTR) - { - pollset_[i] = fds; - return 0; - } - } - return -ENOMEM; -} - -int BusEvent::removePollWaiter(::pollfd* fds) -{ - for (unsigned i = 0; i < MaxPollWaiters; i++) - { - if (fds == pollset_[i]) - { - pollset_[i] = UAVCAN_NULLPTR; - return 0; - } - } - return -EINVAL; -} - BusEvent::BusEvent(CanDriver& can_driver) - : can_driver_(can_driver), - signal_(false) { - std::memset(&file_ops_, 0, sizeof(file_ops_)); - std::memset(pollset_, 0, sizeof(pollset_)); - file_ops_.open = &BusEvent::openTrampoline; - file_ops_.close = &BusEvent::closeTrampoline; - file_ops_.poll = &BusEvent::pollTrampoline; - // TODO: move to init(), add proper error handling - if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) - { - std::abort(); - } + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); } BusEvent::~BusEvent() { - (void)unregister_driver(DevName); + sem_destroy(&sem_); } bool BusEvent::wait(uavcan::MonotonicDuration duration) { - // TODO blocking wait - const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; - while (clock::getMonotonic() < deadline) - { - { - CriticalSectionLocker locker; - if (signal_) - { - signal_ = false; - return true; + if (duration.isPositive()) { + timespec abstime; + + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; + + int ret; + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + if (ret == -1) { // timed out or error + return false; } + return true; } - ::usleep(1000); } return false; } void BusEvent::signalFromInterrupt() { - signal_ = true; // HACK - for (unsigned i = 0; i < MaxPollWaiters; i++) + if (sem_.semcount <= 0) { - ::pollfd* const fd = pollset_[i]; - if (fd != UAVCAN_NULLPTR) - { - fd->revents |= fd->events & POLLIN; - if ((fd->revents != 0) && (fd->sem->semcount <= 0)) - { - (void)sem_post(fd->sem); - } - } + (void)sem_post(&sem_); + } + if (signal_cb_) + { + signal_cb_(); } } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 1e743296f4..1fd21e829a 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -282,6 +282,8 @@ public: * This is designed for use with iface activity LEDs. */ bool hadActivity(); + + BusEvent& updateEvent() { return update_event_; } }; /** diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index b2eac2a65c..600188ce0e 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -66,30 +66,17 @@ public: */ class BusEvent : uavcan::Noncopyable { - static const unsigned MaxPollWaiters = 8; + using SignalCallbackHandler = void(*)(); - ::file_operations file_ops_; - ::pollfd* pollset_[MaxPollWaiters]; - CanDriver& can_driver_; - bool signal_; - - static int openTrampoline(::file* filp); - static int closeTrampoline(::file* filp); - static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup); - - int open(::file* filp); - int close(::file* filp); - int poll(::file* filp, ::pollfd* fds, bool setup); - - int addPollWaiter(::pollfd* fds); - int removePollWaiter(::pollfd* fds); + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; public: - static const char* const DevName; - BusEvent(CanDriver& can_driver); ~BusEvent(); + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + bool wait(uavcan::MonotonicDuration duration); void signalFromInterrupt(); diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index a347dcb0f8..2e30020457 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -138,147 +138,49 @@ void Mutex::unlock() #elif UAVCAN_STM32_NUTTX -const unsigned BusEvent::MaxPollWaiters; -const char* const BusEvent::DevName = "/dev/uavcan/busevent"; - -int BusEvent::openTrampoline(::file* filp) -{ - return static_cast(filp->f_inode->i_private)->open(filp); -} - -int BusEvent::closeTrampoline(::file* filp) -{ - return static_cast(filp->f_inode->i_private)->close(filp); -} - -int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) -{ - return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); -} - -int BusEvent::open(::file* filp) -{ - (void)filp; - return 0; -} - -int BusEvent::close(::file* filp) -{ - (void)filp; - return 0; -} - -int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) -{ - CriticalSectionLocker locker; - int ret = -1; - - if (setup) - { - ret = addPollWaiter(fds); - if (ret == 0) - { - /* - * Two events can be reported via POLLIN: - * - The RX queue is not empty. This event is level-triggered. - * - Transmission complete. This event is edge-triggered. - * FIXME Since TX event is edge-triggered, it can be lost between poll() calls. - */ - fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0); - if (fds->revents != 0) - { - (void)sem_post(fds->sem); - } - } - } - else - { - ret = removePollWaiter(fds); - } - - return ret; -} - -int BusEvent::addPollWaiter(::pollfd* fds) -{ - for (unsigned i = 0; i < MaxPollWaiters; i++) - { - if (pollset_[i] == UAVCAN_NULLPTR) - { - pollset_[i] = fds; - return 0; - } - } - return -ENOMEM; -} - -int BusEvent::removePollWaiter(::pollfd* fds) -{ - for (unsigned i = 0; i < MaxPollWaiters; i++) - { - if (fds == pollset_[i]) - { - pollset_[i] = UAVCAN_NULLPTR; - return 0; - } - } - return -EINVAL; -} - BusEvent::BusEvent(CanDriver& can_driver) - : can_driver_(can_driver) - , signal_(false) { - std::memset(&file_ops_, 0, sizeof(file_ops_)); - std::memset(pollset_, 0, sizeof(pollset_)); - file_ops_.open = &BusEvent::openTrampoline; - file_ops_.close = &BusEvent::closeTrampoline; - file_ops_.poll = &BusEvent::pollTrampoline; - // TODO: move to init(), add proper error handling - if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) - { - std::abort(); - } + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); } BusEvent::~BusEvent() { - (void)unregister_driver(DevName); + sem_destroy(&sem_); } bool BusEvent::wait(uavcan::MonotonicDuration duration) { - // TODO blocking wait - const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; - while (clock::getMonotonic() < deadline) - { - { - CriticalSectionLocker locker; - if (signal_) - { - signal_ = false; - return true; + if (duration.isPositive()) { + timespec abstime; + + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; + + int ret; + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + if (ret == -1) { // timed out or error + return false; } + return true; } - ::usleep(1000); } return false; } void BusEvent::signalFromInterrupt() { - signal_ = true; // HACK - for (unsigned i = 0; i < MaxPollWaiters; i++) + if (sem_.semcount <= 0) { - ::pollfd* const fd = pollset_[i]; - if (fd != UAVCAN_NULLPTR) - { - fd->revents |= fd->events & POLLIN; - if ((fd->revents != 0) && (fd->sem->semcount <= 0)) - { - (void)sem_post(fd->sem); - } - } + (void)sem_post(&sem_); + } + if (signal_cb_) + { + signal_cb_(); } } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index d33eb42084..f3c69a5034 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -56,7 +56,6 @@ #include #include -#include #include #include @@ -79,6 +78,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev(UAVCAN_DEVICE_PATH), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), + ModuleParams(nullptr), _node(can_driver, system_clock, _pool_allocator), _esc_controller(_node), _hardpoint_controller(_node), @@ -87,18 +88,8 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _node_status_monitor(_node), _cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")), _interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")), - _control_latency_perf(perf_alloc(PC_ELAPSED, "uavcan: control latency")), _master_timer(_node) { - _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); - - for (int i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN; ++i) { - _control_subs[i] = -1; - } - int res = pthread_mutex_init(&_node_mutex, nullptr); if (res < 0) { @@ -119,10 +110,11 @@ UavcanNode::~UavcanNode() { fw_server(Stop); - if (_task != -1) { + if (_instance) { /* tell the task we want it to go away */ - _task_should_exit = true; + _task_should_exit.store(true); + ScheduleNow(); unsigned i = 10; @@ -130,31 +122,21 @@ UavcanNode::~UavcanNode() /* wait 5ms - it should wake every 10ms or so worst-case */ usleep(5000); - /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); break; } - } while (_task != -1); + } while (_instance); } // Removing the sensor bridges _sensor_bridges.clear(); - _instance = nullptr; - pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); - // Is it allowed to delete it like that? - if (_mixers != nullptr) { - delete _mixers; - } - perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_control_latency_perf); } int @@ -424,25 +406,14 @@ UavcanNode::get_param(int remote_node_id, const char *name) void UavcanNode::update_params() { - // multicopter air-mode - param_t param_handle = param_find("MC_AIRMODE"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_airmode); - } - - param_handle = param_find("THR_MDL_FAC"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_thr_mdl_factor); - } + _mixing_interface.updateParams(); } int UavcanNode::start_fw_server() { int rv = -1; - _fw_server_action = Busy; + _fw_server_action.store((int)Busy); UavcanServers *_servers = UavcanServers::instance(); if (_servers == nullptr) { @@ -460,7 +431,7 @@ UavcanNode::start_fw_server() } } - _fw_server_action = None; + _fw_server_action.store((int)None); px4_sem_post(&_server_command_sem); return rv; } @@ -469,7 +440,7 @@ int UavcanNode::request_fw_check() { int rv = -1; - _fw_server_action = Busy; + _fw_server_action.store((int)Busy); UavcanServers *_servers = UavcanServers::instance(); if (_servers != nullptr) { @@ -477,7 +448,7 @@ UavcanNode::request_fw_check() rv = 0; } - _fw_server_action = None; + _fw_server_action.store((int)None); px4_sem_post(&_server_command_sem); return rv; } @@ -486,7 +457,7 @@ int UavcanNode::stop_fw_server() { int rv = -1; - _fw_server_action = Busy; + _fw_server_action.store((int)Busy); UavcanServers *_servers = UavcanServers::instance(); if (_servers != nullptr) { @@ -501,7 +472,7 @@ UavcanNode::stop_fw_server() rv = _servers->stop(); } - _fw_server_action = None; + _fw_server_action.store((int)None); px4_sem_post(&_server_command_sem); return rv; } @@ -515,8 +486,8 @@ UavcanNode::fw_server(eServerAction action) case Start: case Stop: case CheckFW: - if (_fw_server_action == None) { - _fw_server_action = action; + if (_fw_server_action.load() == (int)None) { + _fw_server_action.store((int)action); px4_sem_wait(&_server_command_sem); rv = _fw_server_status; } @@ -573,7 +544,7 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - const int node_init_res = _instance->init(node_id); + const int node_init_res = _instance->init(node_id, can->driver.updateEvent()); if (node_init_res < 0) { delete _instance; @@ -582,21 +553,7 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } - /* - * Start the task. Normally it should never exit. - */ - static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = px4_task_spawn_cmd("uavcan", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - StackSize, - static_cast(run_trampoline), - nullptr); - - if (_instance->_task < 0) { - PX4_ERR("start failed: %d", errno); - return -errno; - } + _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); return OK; } @@ -625,8 +582,17 @@ UavcanNode::fill_node_info() _node.setHardwareVersion(hwver); } +void +UavcanNode::busevent_signal_trampoline() +{ + if (_instance) { + // trigger the work queue (Note, this is called from IRQ context) + _instance->ScheduleNow(); + } +} + int -UavcanNode::init(uavcan::NodeID node_id) +UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) { // Do regular cdev init int ret = CDev::init(); @@ -635,6 +601,8 @@ UavcanNode::init(uavcan::NodeID node_id) return ret; } + bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); + _node.setName("org.pixhawk.pixhawk"); _node.setNodeID(node_id); @@ -648,11 +616,6 @@ UavcanNode::init(uavcan::NodeID node_id) return ret; } - { - param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed); - _esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0); - } - ret = _hardpoint_controller.init(); if (ret < 0) { @@ -673,6 +636,22 @@ UavcanNode::init(uavcan::NodeID node_id) PX4_INFO("sensor bridge '%s' init ok", br->get_name()); } + _mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); + _mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT + + // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions + if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE + || UavcanEscController::max_output_value() > (int)UINT16_MAX) { + PX4_ERR("ESC max output value assertion failed"); + return -EINVAL; + } + + _mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value()); + _mixing_interface.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ); + + param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param); + enable_idle_throttle_when_armed(true); + /* Start the Node */ return _node.start(); } @@ -692,27 +671,6 @@ UavcanNode::node_spin_once() } } -/* - add a fd to the list of polled events. This assumes you want - POLLIN for now. - */ -int -UavcanNode::add_poll_fd(int fd) -{ - int ret = _poll_fds_num; - - if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { - errx(1, "uavcan: too many poll fds, exiting"); - } - - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - - return ret; -} - void UavcanNode::handle_time_sync(const uavcan::TimerEvent &) { @@ -753,287 +711,110 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &) _time_sync_master.publish(); } -int -UavcanNode::run() + + +void +UavcanNode::Run() { pthread_mutex_lock(&_node_mutex); - // XXX figure out the output count - _output_count = 2; + if (_output_count == 0) { + // Set up the time synchronization + const int slave_init_res = _time_sync_slave.start(); - actuator_outputs_s outputs{}; + if (slave_init_res < 0) { + PX4_ERR("Failed to start time_sync_slave"); + ScheduleClear(); + return; + } - // Set up the time synchronization - const int slave_init_res = _time_sync_slave.start(); + /* When we have a system wide notion of time update (i.e the transition from the initial + * System RTC setting to the GPS) we would call UAVCAN_DRIVER::clock::setUtc() when that + * happens, but for now we use adjustUtc with a correction of the hrt so that the + * time bases are the same + */ + UAVCAN_DRIVER::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); + _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); + _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); - if (slave_init_res < 0) { - PX4_ERR("Failed to start time_sync_slave"); - _task_should_exit = true; + _node_status_monitor.start(); + _node.setModeOperational(); + + update_params(); + + // XXX figure out the output count + _output_count = 2; } - /* When we have a system wide notion of time update (i.e the transition from the initial - * System RTC setting to the GPS) we would call UAVCAN_DRIVER::clock::setUtc() when that - * happens, but for now we use adjustUtc with a correction of the hrt so that the - * time bases are the same - */ - UAVCAN_DRIVER::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); - _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); - _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); - _node_status_monitor.start(); + perf_begin(_cycle_perf); + perf_count(_interval_perf); - const int busevent_fd = ::open(UAVCAN_DRIVER::BusEvent::DevName, 0); + node_spin_once(); // expected to be non-blocking - if (busevent_fd < 0) { - PX4_ERR("Failed to open %s", UAVCAN_DRIVER::BusEvent::DevName); - _task_should_exit = true; + // Check arming state + const actuator_armed_s &armed = _mixing_interface.mixingOutput().armed(); + enable_idle_throttle_when_armed(!armed.soft_stop); + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + update_params(); } - /* - * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); - * IO multiplexing shall be done here. - */ + switch ((eServerAction)_fw_server_action.load()) { + case Start: + _fw_server_status = start_fw_server(); + break; - _node.setModeOperational(); + case Stop: + _fw_server_status = stop_fw_server(); + break; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - add_poll_fd(busevent_fd); + case CheckFW: + _fw_server_status = request_fw_check(); + break; - update_params(); - - while (!_task_should_exit) { - - perf_begin(_cycle_perf); - perf_count(_interval_perf); - - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - update_params(); - } - - 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(); - _groups_subscribed = _groups_required; - } - - // Mutex is unlocked while the thread is blocked on IO multiplexing - (void)pthread_mutex_unlock(&_node_mutex); - - const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); - - (void)pthread_mutex_lock(&_node_mutex); - - node_spin_once(); // Non-blocking - - bool new_output = false; - - // this would be bad... - if (poll_ret < 0) { - PX4_ERR("poll error %d", errno); - continue; - - } else { - // get controls for required topics - bool controls_updated = false; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - if (_poll_fds[_poll_ids[i]].revents & POLLIN) { - controls_updated = true; - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - } - } - } - - // can we mix? - if (_test_in_progress) { - - memset(&outputs, 0, sizeof(outputs)); - - if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; - outputs.noutputs = _test_motor.motor_number + 1; - } - - new_output = true; - - } else if (controls_updated && (_mixers != nullptr)) { - - // XXX one output group has 8 outputs max, - // but this driver could well serve multiple groups. - unsigned num_outputs_max = 8; - - _mixers->set_thrust_factor(_thr_mdl_factor); - _mixers->set_airmode(_airmode); - - // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - - new_output = true; - } - } - - if (new_output) { - // iterate actuators, checking for valid values - for (uint8_t i = 0; i < outputs.noutputs; i++) { - - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - - // never go below min or max - outputs.output[i] = math::constrain(outputs.output[i], -1.0f, 1.0f); - } - - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); - outputs.timestamp = hrt_absolute_time(); - - // use first valid timestamp_sample for latency tracking - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - const bool required = _groups_required & (1 << i); - const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; - - if (required && (timestamp_sample > 0)) { - perf_set_elapsed(_control_latency_perf, outputs.timestamp - timestamp_sample); - break; - } - } - } - - // Check motor test state - if (_test_motor_sub.updated()) { - _test_motor_sub.copy(&_test_motor); - - // Update the test status and check that we're not locked down - _test_in_progress = (_test_motor.action == test_motor_s::ACTION_RUN); - _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); - } - - // Check arming state - if (_armed_sub.updated()) { - actuator_armed_s armed{}; - _armed_sub.copy(&armed); - - // Update the armed status and check that we're not locked down and motor - // test is not running - bool set_armed = (armed.armed && !armed.lockdown && !armed.manual_lockdown && !_test_in_progress); - - arm_actuators(set_armed); - - if (armed.soft_stop) { - _esc_controller.enable_idle_throttle_when_armed(false); - - } else { - _esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0); - } - } - - perf_end(_cycle_perf); + case None: + default: + break; } - (void)::close(busevent_fd); + perf_end(_cycle_perf); - teardown(); + pthread_mutex_unlock(&_node_mutex); - exit(0); + if (_task_should_exit.load()) { + _mixing_interface.mixingOutput().unregister(); + _mixing_interface.ScheduleClear(); + ScheduleClear(); + teardown(); + _instance = nullptr; + } } -int -UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) +void +UavcanNode::enable_idle_throttle_when_armed(bool value) { - const actuator_controls_s *controls = (actuator_controls_s *)handle; + value &= _idle_throttle_when_armed_param > 0; - input = controls[control_group].control[control_index]; - return 0; + if (value != _idle_throttle_when_armed) { + _mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0); + _idle_throttle_when_armed = value; + } } int UavcanNode::teardown() { px4_sem_post(&_server_command_sem); - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } - } - return 0; } -int -UavcanNode::arm_actuators(bool arm) -{ - _is_armed = arm; - _esc_controller.arm_all_escs(arm); - - return OK; -} - -void -UavcanNode::subscribe() -{ - // Subscribe/unsubscribe to required actuator control groups - uint32_t sub_groups = _groups_required & ~_groups_subscribed; - uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - - // the first fd used by CAN - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (sub_groups & (1 << i)) { - PX4_DEBUG("subscribe to actuator_controls_%d", i); - _control_subs[i] = orb_subscribe(_control_topics[i]); - } - - if (unsub_groups & (1 << i)) { - PX4_DEBUG("unsubscribe from actuator_controls_%d", i); - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } - - if (_control_subs[i] >= 0) { - _poll_ids[i] = add_poll_fd(_control_subs[i]); - orb_set_interval(_control_subs[i], 1000 / UavcanEscController::MAX_RATE_HZ); - } - } -} - int UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) { @@ -1042,66 +823,25 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - case PWM_SERVO_ARM: - arm_actuators(true); - break; - case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: case PWM_SERVO_SET_FORCE_SAFETY_OFF: // these are no-ops, as no safety switch break; - case PWM_SERVO_DISARM: - arm_actuators(false); - break; - case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _output_count; break; case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - } + _mixing_interface.mixingOutput().resetMixerThreadSafe(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - - if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); - } - - if (_mixers == nullptr) { - _groups_required = 0; - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - PX4_ERR("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - ret = -EINVAL; - - } else { - - _mixers->groups_required(_groups_required); - PX4_INFO("Groups required %d", _groups_required); - - int rotor_count = _mixers->get_multirotor_count(); - _esc_controller.set_rotor_count(rotor_count); - PX4_INFO("Number of rotors %d", rotor_count); - } - } + ret = _mixing_interface.mixingOutput().loadMixerThreadSafe(buf, buflen); } break; @@ -1146,6 +886,33 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) return ret; } + +bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + _esc_controller.update_outputs(stop_motors, outputs, num_outputs); + return true; +} + +void UavcanMixingInterface::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(false); + pthread_mutex_unlock(&_node_mutex); +} + +void UavcanMixingInterface::mixerChanged() +{ + int rotor_count = 0; + + if (_mixing_output.mixers()) { + rotor_count = _mixing_output.mixers()->get_multirotor_count(); + } + + _esc_controller.set_rotor_count(rotor_count); +} + void UavcanNode::print_info() { @@ -1185,9 +952,7 @@ UavcanNode::print_info() printf("\n"); // ESC mixer status - printf("ESC actuators control groups: sub: %X / req: %X / fds: %u\n", - (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + _mixing_interface.mixingOutput().printStatus(); printf("\n"); @@ -1210,7 +975,6 @@ UavcanNode::print_info() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_control_latency_perf); (void)pthread_mutex_unlock(&_node_mutex); } @@ -1320,16 +1084,6 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } - if (!std::strcmp(argv[1], "arm")) { - inst->arm_actuators(true); - ::exit(0); - } - - if (!std::strcmp(argv[1], "disarm")) { - inst->arm_actuators(false); - ::exit(0); - } - /* * Parameter setting commands * diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index ee639cf74c..64db78a9d3 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -43,6 +43,8 @@ #pragma once #include +#include +#include #include "uavcan_driver.hpp" #include "uavcan_servers.hpp" @@ -60,31 +62,56 @@ #include #include +#include #include #include -#include -#include -#include #include -#include -#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 +class UavcanNode; -// we add 1 to allow for busevent -#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+1) +/** + * UAVCAN mixing class. + * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling + * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at + * a fixed rate or upon bus updates). + * Both work items are expected to run on the same work queue. + */ +class UavcanMixingInterface : public OutputModuleInterface +{ +public: + UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller) + : OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan), + _node_mutex(node_mutex), + _esc_controller(esc_controller) {} + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + void mixerChanged() override; + + MixingOutput &mixingOutput() { return _mixing_output; } + +protected: + void Run() override; +private: + friend class UavcanNode; + pthread_mutex_t &_node_mutex; + UavcanEscController &_esc_controller; + MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; +}; /** * A UAVCAN node. */ -class UavcanNode : public cdev::CDev +class UavcanNode : public cdev::CDev, public px4::ScheduledWorkItem, public ModuleParams { static constexpr unsigned MaxBitRatePerSec = 1000000; static constexpr unsigned bitPerFrame = 148; static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned PollTimeoutMs = 3; + static constexpr unsigned ScheduleIntervalMs = 3; /* @@ -92,19 +119,18 @@ class UavcanNode : public cdev::CDev * At 1Mbit there is approximately one CAN frame every 145 uS. * The number of buffers sets how long you can go without calling * node_spin_xxxx. Since our task is the only one running and the - * driver will light the fd when there is a CAN frame we can nun with + * driver will light the callback when there is a CAN frame we can nun with * a minimum number of buffers to conserver memory. Each buffer is * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate * of ~1 mS * 1000000/200 */ - static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 2400; + static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs; // At public: typedef UAVCAN_DRIVER::CanInitHelper CanInitHelper; - enum eServerAction {None, Start, Stop, CheckFW, Busy}; + enum eServerAction : int {None, Start, Stop, CheckFW, Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -116,15 +142,8 @@ public: uavcan::Node<> &get_node() { return _node; } - // TODO: move the actuator mixing stuff into the ESC controller class - static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - - void subscribe(); - int teardown(); - int arm_actuators(bool arm); - void print_info(); void shrink(); @@ -141,13 +160,14 @@ public: int get_param(int remote_node_id, const char *name); int reset_node(int remote_node_id); + static void busevent_signal_trampoline(); + +protected: + void Run() override; private: void fill_node_info(); - int init(uavcan::NodeID node_id); + int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events); 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(); @@ -160,16 +180,14 @@ private: void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; } void free_setget_response(void) { _setget_response = nullptr; } - 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{None}; + void enable_idle_throttle_when_armed(bool value); + + px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver + px4::atomic _fw_server_action{None}; int _fw_server_status{-1}; bool _is_armed{false}; ///< the arming status of the actuators on the bus - test_motor_s _test_motor{}; - bool _test_in_progress{false}; - unsigned _output_count{0}; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer @@ -177,9 +195,10 @@ private: uavcan_node::Allocator _pool_allocator; uavcan::Node<> _node; ///< library instance - pthread_mutex_t _node_mutex{}; + pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; + UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller}; UavcanHardpointController _hardpoint_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave; @@ -187,31 +206,15 @@ private: List _sensor_bridges; ///< List of active sensor bridges - MixerGroup *_mixers{nullptr}; ITxQueueInjector *_tx_injector{nullptr}; - uint32_t _groups_required{0}; - uint32_t _groups_subscribed{0}; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {}; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {}; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {}; - pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] {}; - unsigned _poll_fds_num{0}; - int32_t _idle_throttle_when_armed{0}; + bool _idle_throttle_when_armed{false}; + int32_t _idle_throttle_when_armed_param{0}; - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _test_motor_sub{ORB_ID(test_motor)}; perf_counter_t _cycle_perf; perf_counter_t _interval_perf; - perf_counter_t _control_latency_perf; - - Mixer::Airmode _airmode{Mixer::Airmode::disabled}; - float _thr_mdl_factor{0.0f}; - - // index into _poll_fds for each _control_subs handle - uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {}; void handle_time_sync(const uavcan::TimerEvent &); diff --git a/src/drivers/uavcanesc/uavcanesc_main.cpp b/src/drivers/uavcanesc/uavcanesc_main.cpp index 258b6db4e7..7e2105bc66 100644 --- a/src/drivers/uavcanesc/uavcanesc_main.cpp +++ b/src/drivers/uavcanesc/uavcanesc_main.cpp @@ -57,6 +57,8 @@ #include "boot_app_shared.h" +using namespace time_literals; + /** * * Implements basic functionality of UAVCAN esc. @@ -99,6 +101,10 @@ UavcanEsc::UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &syste std::abort(); } + px4_sem_init(&_sem, 0, 0); + /* semaphore use case is a signal */ + px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); + } UavcanEsc::~UavcanEsc() @@ -123,7 +129,7 @@ UavcanEsc::~UavcanEsc() } _instance = nullptr; - + px4_sem_destroy(&_sem); } int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -163,7 +169,7 @@ int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate) } - const int node_init_res = _instance->init(node_id); + const int node_init_res = _instance->init(node_id, can.driver.updateEvent()); if (node_init_res < 0) { delete _instance; @@ -255,7 +261,7 @@ void UavcanEsc::cb_beginfirmware_update(const uavcan::ReceivedDataStructure= UAVCAN_NUM_POLL_FDS) { - errx(1, "uavcan: too many poll fds, exiting"); + if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) { + return; } - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - return ret; + px4_sem_post(sem); } +void +UavcanEsc::busevent_signal_trampoline() +{ + if (_instance) { + signal_callback(&_instance->_sem); + } +} + int UavcanEsc::run() { @@ -338,54 +348,28 @@ int UavcanEsc::run() (void)pthread_mutex_lock(&_node_mutex); - const unsigned PollTimeoutMs = 50; - - const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); - - if (busevent_fd < 0) { - PX4_WARN("Failed to open %s", uavcan_stm32::BusEvent::DevName); - _task_should_exit = true; - } - - /* If we had an RTC we would call uavcan_stm32::clock::setUtc() - * but for now we use adjustUtc with a correction of 0 - */ -// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); - _node.setModeOperational(); - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - add_poll_fd(busevent_fd); + hrt_call timer_call{}; + hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem); while (!_task_should_exit) { // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); - const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); - + while (px4_sem_wait(&_sem) != 0); (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking + // Do Something - // this would be bad... - if (poll_ret < 0) { - PX4_ERR("poll error %d", errno); - continue; - - } else { - // Do Something - } } + hrt_cancel(&timer_call); teardown(); - PX4_WARN("exiting."); + (void)pthread_mutex_unlock(&_node_mutex); exit(0); } diff --git a/src/drivers/uavcanesc/uavcanesc_main.hpp b/src/drivers/uavcanesc/uavcanesc_main.hpp index 7c6fb1c040..523c672e27 100644 --- a/src/drivers/uavcanesc/uavcanesc_main.hpp +++ b/src/drivers/uavcanesc/uavcanesc_main.hpp @@ -115,12 +115,13 @@ public: private: void fill_node_info(); - int init(uavcan::NodeID node_id); + int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events); void node_spin_once(); int run(); - int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + static void busevent_signal_trampoline(); + px4_sem_t _sem; ///< semaphore for scheduling the task int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -128,9 +129,6 @@ private: Node _node; ///< library instance pthread_mutex_t _node_mutex; - pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; - unsigned _poll_fds_num = 0; - typedef uavcan::MethodBinder &, uavcan::ServiceResponseDataStructure &)> diff --git a/src/drivers/uavcannode/uavcannode_main.cpp b/src/drivers/uavcannode/uavcannode_main.cpp index 61c565c7eb..f378f4d45e 100644 --- a/src/drivers/uavcannode/uavcannode_main.cpp +++ b/src/drivers/uavcannode/uavcannode_main.cpp @@ -64,6 +64,8 @@ __END_DECLS #include "boot_app_shared.h" +using namespace time_literals; + /** * @file uavcan_main.cpp * @@ -124,6 +126,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys std::abort(); } + px4_sem_init(&_sem, 0, 0); + /* semaphore use case is a signal */ + px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); } UavcanNode::~UavcanNode() @@ -148,7 +153,7 @@ UavcanNode::~UavcanNode() } _instance = nullptr; - + px4_sem_destroy(&_sem); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -189,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) resources("Before _instance->init:"); - const int node_init_res = _instance->init(node_id); + const int node_init_res = _instance->init(node_id, can.driver.updateEvent()); resources("After _instance->init:"); if (node_init_res < 0) { @@ -282,7 +287,7 @@ void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure= UAVCAN_NUM_POLL_FDS) { - errx(1, "uavcan: too many poll fds, exiting"); + if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) { + return; } - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - return ret; + px4_sem_post(sem); } +void +UavcanNode::busevent_signal_trampoline() +{ + if (_instance) { + signal_callback(&_instance->_sem); + } +} + + + int UavcanNode::run() { @@ -381,52 +392,26 @@ int UavcanNode::run() _task_should_exit = true; } - const unsigned PollTimeoutMs = 50; - - const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); - - if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); - _task_should_exit = true; - } - - /* If we had an RTC we would call uavcan_stm32::clock::setUtc() - * but for now we use adjustUtc with a correction of 0 - */ -// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); - _node.setModeOperational(); - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - add_poll_fd(busevent_fd); - uint32_t start_tick = clock_systimer(); + hrt_call timer_call{}; + hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem); + while (!_task_should_exit) { // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); - const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); - + while (px4_sem_wait(&_sem) != 0); (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking - // this would be bad... - if (poll_ret < 0) { - PX4_ERR("poll error %d", errno); - continue; + // Do Something - } else { - // Do Something - } if (clock_systimer() - start_tick > TICK_PER_SEC) { start_tick = clock_systimer(); @@ -455,8 +440,9 @@ int UavcanNode::run() } + hrt_cancel(&timer_call); teardown(); - warnx("exiting."); + (void)pthread_mutex_unlock(&_node_mutex); exit(0); } diff --git a/src/drivers/uavcannode/uavcannode_main.hpp b/src/drivers/uavcannode/uavcannode_main.hpp index 59ff60641c..5e2a1f6b1d 100644 --- a/src/drivers/uavcannode/uavcannode_main.hpp +++ b/src/drivers/uavcannode/uavcannode_main.hpp @@ -118,12 +118,13 @@ public: private: void fill_node_info(); - int init(uavcan::NodeID node_id); + int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events); void node_spin_once(); int run(); - int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + static void busevent_signal_trampoline(); + px4_sem_t _sem; ///< semaphore for scheduling the task int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -132,9 +133,6 @@ private: pthread_mutex_t _node_mutex; uavcan::GlobalTimeSyncSlave _time_sync_slave; - pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; - unsigned _poll_fds_num = 0; - typedef uavcan::MethodBinder &, uavcan::ServiceResponseDataStructure &)>