From 375fc4a75ce8b63fe2847f99ce2ab5bf6b483cd0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 28 Oct 2019 19:57:50 -0400 Subject: [PATCH] uavcan module cleanup - move most orb to uORB::Publication and uORB::Subscription - update legacy message handling (warn to PX4_INFO, PX4_WARN, PX4_ERR) - add perf counters - sensors/mag support newer `uavcan::equipment::ahrs::MagneticFieldStrength2` message - sensors/gps support `uavcan::equipment::gnss::Auxiliary` for hdop and vdop - sensors delete obsolete ioctl and read methods - use PublicationMulti for actuator_outputs and esc_reports (to coexist with other output modules) - add GNSS parameter metadata (parameters_injected.xml) --- src/drivers/uavcan/actuators/esc.cpp | 57 ++-- src/drivers/uavcan/actuators/esc.hpp | 34 ++- src/drivers/uavcan/actuators/hardpoint.cpp | 19 +- src/drivers/uavcan/allocator.hpp | 4 +- src/drivers/uavcan/sensors/baro.cpp | 48 +-- src/drivers/uavcan/sensors/baro.hpp | 9 +- src/drivers/uavcan/sensors/gnss.cpp | 212 +++++++------ src/drivers/uavcan/sensors/gnss.hpp | 36 ++- src/drivers/uavcan/sensors/mag.cpp | 74 ++--- src/drivers/uavcan/sensors/mag.hpp | 16 +- src/drivers/uavcan/sensors/sensor_bridge.cpp | 11 +- src/drivers/uavcan/sensors/sensor_bridge.hpp | 4 +- src/drivers/uavcan/uavcan_main.cpp | 264 ++++++++--------- src/drivers/uavcan/uavcan_main.hpp | 113 +++---- src/drivers/uavcan/uavcan_servers.cpp | 294 +++++++++---------- src/drivers/uavcan/uavcan_servers.hpp | 13 +- src/lib/drivers/device/Device.hpp | 92 +++--- src/lib/drivers/device/nuttx/I2C.hpp | 4 +- src/lib/drivers/device/nuttx/SPI.hpp | 4 +- src/lib/drivers/device/posix/I2C.hpp | 4 +- src/lib/drivers/device/posix/SPI.hpp | 4 +- src/lib/parameters/parameters_injected.xml | 66 ++++- 22 files changed, 705 insertions(+), 677 deletions(-) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index de4a964b8f..61683a6cdb 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -52,14 +52,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _orb_timer(node) { _uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY); - - if (_perfcnt_invalid_input == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); - } - - if (_perfcnt_scaling_error == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); - } } UavcanEscController::~UavcanEscController() @@ -68,13 +60,14 @@ UavcanEscController::~UavcanEscController() perf_free(_perfcnt_scaling_error); } -int UavcanEscController::init() +int +UavcanEscController::init() { // ESC status subscription int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); if (res < 0) { - warnx("ESC status sub failed %i", res); + PX4_ERR("ESC status sub failed %i", res); return res; } @@ -85,11 +78,13 @@ int UavcanEscController::init() return res; } -void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +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)) { + perf_count(_perfcnt_invalid_input); return; } @@ -111,9 +106,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) */ uavcan::equipment::esc::RawCommand msg; - actuator_outputs_s actuator_outputs = {}; + actuator_outputs_s actuator_outputs{}; actuator_outputs.noutputs = num_outputs; - actuator_outputs.timestamp = hrt_absolute_time(); 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; @@ -167,21 +161,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ - (void)_uavcan_pub_raw_cmd.broadcast(msg); + _uavcan_pub_raw_cmd.broadcast(msg); // Publish actuator outputs - if (_actuator_outputs_pub != nullptr) { - orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs); - - } else { - int instance; - _actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs, - &instance, ORB_PRIO_DEFAULT); - } - + actuator_outputs.timestamp = hrt_absolute_time(); + _actuator_outputs_pub.publish(actuator_outputs); } -void UavcanEscController::arm_all_escs(bool arm) +void +UavcanEscController::arm_all_escs(bool arm) { if (arm) { _armed_mask = -1; @@ -191,7 +179,8 @@ void UavcanEscController::arm_all_escs(bool arm) } } -void UavcanEscController::arm_single_esc(int num, bool arm) +void +UavcanEscController::arm_single_esc(int num, bool arm) { if (arm) { _armed_mask = MOTOR_BIT(num); @@ -201,7 +190,8 @@ void UavcanEscController::arm_single_esc(int num, bool arm) } } -void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +void +UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) { auto &ref = _esc_status.esc[msg.esc_index]; @@ -217,7 +207,8 @@ 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.timestamp = hrt_absolute_time(); _esc_status.esc_count = _rotor_count; @@ -225,18 +216,14 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; _esc_status.esc_online_flags = UavcanEscController::check_escs_status(); - if (_esc_status_pub != nullptr) { - (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); - - } else { - _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); - } + _esc_status_pub.publish(_esc_status); } -uint8_t UavcanEscController::check_escs_status() +uint8_t +UavcanEscController::check_escs_status() { int esc_status_flags = 0; - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 6001b5f5f6..471524f5d5 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -47,12 +47,12 @@ #include #include #include -#include -#include +#include +#include #include +#include #include - class UavcanEscController { public: @@ -95,17 +95,19 @@ private: static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest typedef uavcan::MethodBinder&)> - StatusCbBinder; + void (UavcanEscController::*)(const uavcan::ReceivedDataStructure&)> StatusCbBinder; - typedef uavcan::MethodBinder - TimerCbBinder; + 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)}; - bool _armed = false; - bool _run_at_idle_throttle_when_armed = false; - esc_status_s _esc_status = {}; - orb_advert_t _esc_status_pub = nullptr; - orb_advert_t _actuator_outputs_pub = nullptr; uint8_t _rotor_count = 0; /* @@ -120,12 +122,12 @@ private: /* * ESC states */ - uint32_t _armed_mask = 0; - uint8_t _max_number_of_nonzero_outputs = 0; + 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"); + 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/actuators/hardpoint.cpp b/src/drivers/uavcan/actuators/hardpoint.cpp index 2fc7599d00..e1d9e52115 100644 --- a/src/drivers/uavcan/actuators/hardpoint.cpp +++ b/src/drivers/uavcan/actuators/hardpoint.cpp @@ -48,13 +48,13 @@ UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) : _uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower); } - UavcanHardpointController::~UavcanHardpointController() { } -int UavcanHardpointController::init() +int +UavcanHardpointController::init() { /* * Setup timer and call back function for periodic updates @@ -63,7 +63,8 @@ int UavcanHardpointController::init() return 0; } -void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command) +void +UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command) { _cmd.command = command; _cmd.hardpoint_id = hardpoint_id; @@ -71,7 +72,7 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma /* * Publish the command message to the bus */ - (void)_uavcan_pub_raw_cmd.broadcast(_cmd); + _uavcan_pub_raw_cmd.broadcast(_cmd); /* * Start the periodic update timer after a command is set @@ -81,10 +82,10 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma } } -void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &) + +void +UavcanHardpointController::periodic_update(const uavcan::TimerEvent &) { - /* - * Broadcast command at MAX_RATE_HZ - */ - (void)_uavcan_pub_raw_cmd.broadcast(_cmd); + // Broadcast command at MAX_RATE_HZ + _uavcan_pub_raw_cmd.broadcast(_cmd); } diff --git a/src/drivers/uavcan/allocator.hpp b/src/drivers/uavcan/allocator.hpp index 4d2fe00502..3f11ee558d 100644 --- a/src/drivers/uavcan/allocator.hpp +++ b/src/drivers/uavcan/allocator.hpp @@ -61,8 +61,8 @@ struct Allocator : public uavcan::HeapBasedPoolAllocator 0) { - warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", - getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); + PX4_ERR("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", + getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); } } }; diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 559cc091aa..6a515a6a55 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -44,8 +44,7 @@ const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), - _sub_air_temperature_data(node), - _reports(2, sizeof(sensor_baro_s)) + _sub_air_temperature_data(node) { } int UavcanBarometerBridge::init() @@ -73,49 +72,15 @@ int UavcanBarometerBridge::init() return 0; } -ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_baro_s); - sensor_baro_s *baro_buf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - while (count--) { - if (_reports.get(baro_buf)) { - ret += sizeof(*baro_buf); - baro_buf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; -} - -int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case SENSORIOCSPOLLRATE: { - // not supported yet, pretend that everything is ok - return OK; - } - - default: { - return CDev::ioctl(filp, cmd, arg); - } - } -} - -void UavcanBarometerBridge::air_temperature_sub_cb(const +void +UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { last_temperature_kelvin = msg.static_temperature; } -void UavcanBarometerBridge::air_pressure_sub_cb(const +void +UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg) { sensor_baro_s report{}; @@ -135,8 +100,5 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const /* TODO get device ID for sensor */ report.device_id = 0; - // add to the ring buffer - _reports.force(&report); - publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/baro.hpp b/src/drivers/uavcan/sensors/baro.hpp index b3e8b14aa6..53661c0b27 100644 --- a/src/drivers/uavcan/sensors/baro.hpp +++ b/src/drivers/uavcan/sensors/baro.hpp @@ -39,13 +39,10 @@ #include "sensor_bridge.hpp" #include -#include #include #include -class RingBuffer; - class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -58,8 +55,6 @@ public: int init() override; private: - ssize_t read(struct file *filp, char *buffer, size_t buflen); - int ioctl(struct file *filp, int cmd, unsigned long arg) override; void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -77,8 +72,6 @@ private: uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; - ringbuffer::RingBuffer _reports; - - float last_temperature_kelvin = 0.0f; + float last_temperature_kelvin{0.0f}; }; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 181399fd71..e0c384783a 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -47,24 +47,22 @@ #include #include +using namespace time_literals; + const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), + _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), _pub_fix2(node), - _orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)), - _report_pub(nullptr) + _orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)) { } -UavcanGnssBridge::~UavcanGnssBridge() -{ - (void) orb_unsubscribe(_orb_sub_gnss); -} - -int UavcanGnssBridge::init() +int +UavcanGnssBridge::init() { int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower); @@ -73,6 +71,13 @@ int UavcanGnssBridge::init() return res; } + res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb)); + + if (res < 0) { + PX4_WARN("GNSS auxiliary sub failed %i", res); + return res; + } + res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { @@ -87,18 +92,19 @@ int UavcanGnssBridge::init() return res; } - _orb_to_uavcan_pub_timer.startPeriodic( - uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ)); + _orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ)); return res; } -unsigned UavcanGnssBridge::get_num_redundant_channels() const +unsigned +UavcanGnssBridge::get_num_redundant_channels() const { return (_receiver_node_id < 0) ? 0 : 1; } -void UavcanGnssBridge::print_status() const +void +UavcanGnssBridge::print_status() const { printf("RX errors: %d, using old Fix: %d, receiver node id: ", _sub_fix.getFailureCount(), int(_old_fix_subscriber_active)); @@ -111,7 +117,17 @@ void UavcanGnssBridge::print_status() const } } -void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +void +UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // store latest hdop and vdop for use in process_fixx(); + _last_gnss_auxiliary_timestamp = hrt_absolute_time(); + _last_gnss_auxiliary_hdop = msg.hdop; + _last_gnss_auxiliary_vdop = msg.vdop; +} + +void +UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { const bool valid_pos_cov = !msg.position_covariance.empty(); const bool valid_vel_cov = !msg.velocity_covariance.empty(); @@ -125,10 +141,11 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +void +UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg) { if (_old_fix_subscriber_active) { - PX4_WARN("GNSS Fix2 message detected, disabling support for the old Fix message"); + PX4_INFO("GNSS Fix2 message detected, disabling support for the old Fix message"); _sub_fix.stop(); _old_fix_subscriber_active = false; _receiver_node_id = -1; @@ -139,8 +156,8 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, - const float (&pos_cov)[9], - const float (&vel_cov)[9], - const bool valid_pos_cov, - const bool valid_vel_cov) + const float (&pos_cov)[9], const float (&vel_cov)[9], + const bool valid_pos_cov, const bool valid_vel_cov) { // This bridge does not support redundant GNSS receivers yet. if (_receiver_node_id < 0) { _receiver_node_id = msg.getSrcNodeID().get(); - PX4_WARN("GNSS receiver node ID: %d", _receiver_node_id); + PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id); } else { if (_receiver_node_id != msg.getSrcNodeID().get()) { @@ -259,7 +274,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure } } - auto report = ::vehicle_gps_position_s(); + vehicle_gps_position_s report{}; /* * FIXME HACK @@ -329,107 +344,78 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.timestamp_time_relative = 0; - const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); + const uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); switch (msg.gnss_time_standard) { - case FixType::GNSS_TIME_STANDARD_UTC: { - report.time_utc_usec = gnss_ts_usec; - break; + case FixType::GNSS_TIME_STANDARD_UTC: + report.time_utc_usec = gnss_ts_usec; + break; + + case FixType::GNSS_TIME_STANDARD_GPS: + if (msg.num_leap_seconds > 0) { + report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9; } - case FixType::GNSS_TIME_STANDARD_GPS: { - if (msg.num_leap_seconds > 0) { - report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9; - } + break; - break; + case FixType::GNSS_TIME_STANDARD_TAI: + if (msg.num_leap_seconds > 0) { + report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10; } - case FixType::GNSS_TIME_STANDARD_TAI: { - if (msg.num_leap_seconds > 0) { - report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10; - } + break; - break; - } - - default: { - break; - } + default: + break; } -//TODO px4_clock_settime does nothing on the Snapdragon platform -#ifndef __PX4_QURT - // If we haven't already done so, set the system clock using GPS data if (valid_pos_cov && !_system_clock_set) { - timespec ts; - memset(&ts, 0, sizeof(ts)); + timespec ts{}; + // get the whole microseconds ts.tv_sec = report.time_utc_usec / 1000000ULL; + // get the remainder microseconds and convert to nanoseconds ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000; + px4_clock_settime(CLOCK_REALTIME, &ts); + _system_clock_set = true; } -#endif - report.satellites_used = msg.sats_used; - // Using PDOP for HDOP and VDOP - // Relevant discussion: https://github.com/PX4/Firmware/issues/5153 - report.hdop = msg.pdop; - report.vdop = msg.pdop; + if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) { + report.hdop = _last_gnss_auxiliary_hdop; + report.vdop = _last_gnss_auxiliary_vdop; + + } else { + // Using PDOP for HDOP and VDOP + // Relevant discussion: https://github.com/PX4/Firmware/issues/5153 + report.hdop = msg.pdop; + report.vdop = msg.pdop; + } report.heading = NAN; report.heading_offset = NAN; // Publish to a multi-topic - int32_t gps_orb_instance; - orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance, - ORB_PRIO_HIGH); + _gps_pub.publish(report); // Doing less time critical stuff here if (_orb_to_uavcan_pub_timer.isRunning()) { _orb_to_uavcan_pub_timer.stop(); - PX4_WARN("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers"); + PX4_INFO("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers"); } } -void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &) +void +UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &) { - if (_orb_sub_gnss < 0) { - // ORB subscription stops working if this is relocated into init() - _orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position)); + vehicle_gps_position_s orb_msg{}; - if (_orb_sub_gnss < 0) { - PX4_WARN("GNSS ORB sub errno %d", errno); - return; - } - - PX4_WARN("GNSS ORB fd %d", _orb_sub_gnss); - } - - { - bool updated = false; - const int res = orb_check(_orb_sub_gnss, &updated); - - if (res < 0) { - PX4_WARN("GNSS ORB check err %d %d", res, errno); - return; - } - - if (!updated) { - return; - } - } - - auto orb_msg = ::vehicle_gps_position_s(); - const int res = orb_copy(ORB_ID(vehicle_gps_position), _orb_sub_gnss, &orb_msg); - - if (res < 0) { - PX4_WARN("GNSS ORB read errno %d", errno); + if (!_orb_sub_gnss.update(&orb_msg)) { return; } @@ -462,5 +448,5 @@ void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &) msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop; // this is a hack :( // Publishing now - (void) _pub_fix2.broadcast(msg); + _pub_fix2.broadcast(msg); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 0717983d7e..cef060148c 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 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 @@ -44,10 +44,12 @@ #pragma once -#include +#include +#include #include #include +#include #include #include @@ -61,7 +63,7 @@ public: static const char *const NAME; UavcanGnssBridge(uavcan::INode &node); - ~UavcanGnssBridge(); + ~UavcanGnssBridge() = default; const char *get_name() const override { return NAME; } @@ -75,18 +77,21 @@ private: /** * GNSS fix message will be reported via this callback. */ + void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); template void process_fixx(const uavcan::ReceivedDataStructure &msg, - const float (&pos_cov)[9], - const float (&vel_cov)[9], - const bool valid_pos_cov, - const bool valid_vel_cov); + const float (&pos_cov)[9], const float (&vel_cov)[9], + const bool valid_pos_cov, const bool valid_vel_cov); void broadcast_from_orb(const uavcan::TimerEvent &); + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + AuxiliaryCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > FixCbBinder; @@ -100,17 +105,24 @@ private: TimerCbBinder; uavcan::INode &_node; + + uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; + uavcan::Publisher _pub_fix2; + uavcan::TimerEventForwarder _orb_to_uavcan_pub_timer; - int _receiver_node_id = -1; - bool _old_fix_subscriber_active = true; + uint64_t _last_gnss_auxiliary_timestamp{0}; + float _last_gnss_auxiliary_hdop{0.0f}; + float _last_gnss_auxiliary_vdop{0.0f}; - orb_advert_t _report_pub; ///< uORB pub for gnss position + uORB::PublicationMulti _gps_pub{ORB_ID(vehicle_gps_position), ORB_PRIO_DEFAULT}; + uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)}; - int _orb_sub_gnss = -1; ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN + int _receiver_node_id{-1}; + bool _old_fix_subscriber_active{true}; + bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? - bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data? }; diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index 4b38c80f4f..2756be960d 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -44,7 +44,8 @@ const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), - _sub_mag(node) + _sub_mag(node), + _sub_mag2(node) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why? @@ -53,7 +54,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : _scale.z_scale = 1.0F; } -int UavcanMagnetometerBridge::init() +int +UavcanMagnetometerBridge::init() { int res = device::CDev::init(); @@ -64,40 +66,22 @@ int UavcanMagnetometerBridge::init() res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { - DEVICE_LOG("failed to start uavcan sub: %d", res); + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + + res = _sub_mag2.start(Mag2CbBinder(this, &UavcanMagnetometerBridge::mag2_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub2: %d", res); return res; } return 0; } -ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) -{ - static uint64_t last_read = 0; - struct mag_report *mag_buf = reinterpret_cast(buffer); - - /* buffer must be large enough */ - unsigned count = buflen / sizeof(struct mag_report); - - if (count < 1) { - return -ENOSPC; - } - - if (last_read < _report.timestamp) { - /* copy report */ - lock(); - *mag_buf = _report; - last_read = _report.timestamp; - unlock(); - return sizeof(struct mag_report); - - } else { - /* no new data available, warn caller */ - return -EAGAIN; - } -} - -int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +int +UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -127,9 +111,33 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar } } -void UavcanMagnetometerBridge::mag_sub_cb(const - uavcan::ReceivedDataStructure - &msg) +void +UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + lock(); + /* + * FIXME HACK + * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. + * It stopped working when the time sync feature has been introduced, because it caused libuavcan + * to use an independent time source (based on hardware TIM5) instead of HRT. + * The proper solution is to be developed. + */ + _report.timestamp = hrt_absolute_time(); + _report.device_id = _device_id.devid; + _report.is_external = true; + + _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); +} + +void +UavcanMagnetometerBridge::mag2_sub_cb(const + uavcan::ReceivedDataStructure &msg) { lock(); /* diff --git a/src/drivers/uavcan/sensors/mag.hpp b/src/drivers/uavcan/sensors/mag.hpp index 8dc5ddd933..04e1208270 100644 --- a/src/drivers/uavcan/sensors/mag.hpp +++ b/src/drivers/uavcan/sensors/mag.hpp @@ -38,9 +38,11 @@ #pragma once #include "sensor_bridge.hpp" + #include #include +#include class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { @@ -54,17 +56,25 @@ public: int init() override; 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 mag2_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, void (UavcanMagnetometerBridge::*) (const uavcan::ReceivedDataStructure &) > MagCbBinder; + typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, + void (UavcanMagnetometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + Mag2CbBinder; + uavcan::Subscriber _sub_mag; - struct mag_calibration_s _scale = {}; - mag_report _report = {}; + uavcan::Subscriber _sub_mag2; + + mag_calibration_s _scale{}; + sensor_mag_s _report{}; }; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index bdb0a42bd9..b20a3dc1e9 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -70,10 +70,9 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() delete [] _channels; } -void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) +void +UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { - assert(report != nullptr); - Channel *channel = nullptr; // Checking if such channel already exists @@ -141,7 +140,8 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) (void)orb_publish(_orb_topic, channel->orb_advert, report); } -unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +unsigned +UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; @@ -154,7 +154,8 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const return out; } -void UavcanCDevSensorBridgeBase::print_status() const +void +UavcanCDevSensorBridgeBase::print_status() const { printf("devname: %s\n", _class_devname); diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index f2bde22958..921c358493 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -51,7 +51,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode #include -#include #include #include @@ -77,23 +76,20 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; + UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev(UAVCAN_DEVICE_PATH), _node(can_driver, system_clock, _pool_allocator), - _node_mutex(), _esc_controller(_node), _hardpoint_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), _node_status_monitor(_node), - _perf_control_latency(perf_alloc(PC_ELAPSED, "uavcan control latency")), - _master_timer(_node), - _setget_response(0) + _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) { - _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); @@ -121,7 +117,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys UavcanNode::~UavcanNode() { - fw_server(Stop); if (_task != -1) { @@ -144,8 +139,6 @@ UavcanNode::~UavcanNode() } while (_task != -1); } - (void)orb_unsubscribe(_armed_sub); - (void)orb_unsubscribe(_test_motor_sub); (void)orb_unsubscribe(_actuator_direct_sub); // Removing the sensor bridges @@ -161,10 +154,13 @@ UavcanNode::~UavcanNode() delete _mixers; } - perf_free(_perf_control_latency); + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_control_latency_perf); } -int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) +int +UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) { int rv = -1; @@ -185,7 +181,8 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) return rv; } -int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp) +int +UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp) { if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { return std::printf("name: %s %lld\n", resp.name.c_str(), @@ -207,7 +204,8 @@ int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp) return -1; } -void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) +void +UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) { uavcan::protocol::param::ExecuteOpcode::Response resp; _callback_success = result.isSuccessful(); @@ -215,7 +213,8 @@ void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) +void +UavcanNode::cb_restart(const uavcan::ServiceCallResult &result) { uavcan::protocol::RestartNode::Response resp; _callback_success = result.isSuccessful(); @@ -246,7 +246,8 @@ void UavcanNode::cb_restart(const uavcan::ServiceCallResult &result) +void +UavcanNode::cb_setget(const uavcan::ServiceCallResult &result) { _callback_success = result.isSuccessful(); *_setget_response = result.getResponse(); } -int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req) +int +UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req) { if (name != nullptr) { req.name = name; @@ -331,7 +334,8 @@ int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::pro return call_res; } -int UavcanNode::set_param(int remote_node_id, const char *name, char *value) +int +UavcanNode::set_param(int remote_node_id, const char *name, char *value) { uavcan::protocol::param::GetSet::Request req; uavcan::protocol::param::GetSet::Response resp; @@ -398,7 +402,8 @@ int UavcanNode::set_param(int remote_node_id, const char *name, char *value) return rv; } -int UavcanNode::get_param(int remote_node_id, const char *name) +int +UavcanNode::get_param(int remote_node_id, const char *name) { uavcan::protocol::param::GetSet::Request req; uavcan::protocol::param::GetSet::Response resp; @@ -418,7 +423,8 @@ int UavcanNode::get_param(int remote_node_id, const char *name) return rv; } -void UavcanNode::update_params() +void +UavcanNode::update_params() { // multicopter air-mode param_t param_handle = param_find("MC_AIRMODE"); @@ -434,7 +440,8 @@ void UavcanNode::update_params() } } -int UavcanNode::start_fw_server() +int +UavcanNode::start_fw_server() { int rv = -1; _fw_server_action = Busy; @@ -460,11 +467,12 @@ int UavcanNode::start_fw_server() return rv; } -int UavcanNode::request_fw_check() +int +UavcanNode::request_fw_check() { int rv = -1; _fw_server_action = Busy; - UavcanServers *_servers = UavcanServers::instance(); + UavcanServers *_servers = UavcanServers::instance(); if (_servers != nullptr) { _servers->requestCheckAllNodesFirmwareAndUpdate(); @@ -474,14 +482,14 @@ int UavcanNode::request_fw_check() _fw_server_action = None; px4_sem_post(&_server_command_sem); return rv; - } -int UavcanNode::stop_fw_server() +int +UavcanNode::stop_fw_server() { int rv = -1; _fw_server_action = Busy; - UavcanServers *_servers = UavcanServers::instance(); + UavcanServers *_servers = UavcanServers::instance(); if (_servers != nullptr) { /* @@ -500,8 +508,8 @@ int UavcanNode::stop_fw_server() return rv; } - -int UavcanNode::fw_server(eServerAction action) +int +UavcanNode::fw_server(eServerAction action) { int rv = -EAGAIN; @@ -525,8 +533,8 @@ int UavcanNode::fw_server(eServerAction action) return rv; } - -int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) +int +UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { PX4_WARN("Already started"); @@ -580,8 +588,12 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * 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); + _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); @@ -591,7 +603,8 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return OK; } -void UavcanNode::fill_node_info() +void +UavcanNode::fill_node_info() { /* software version */ uavcan::protocol::SoftwareVersion swver; @@ -614,13 +627,11 @@ void UavcanNode::fill_node_info() _node.setHardwareVersion(hwver); } - -int UavcanNode::init(uavcan::NodeID node_id) +int +UavcanNode::init(uavcan::NodeID node_id) { - int ret = -1; - // Do regular cdev init - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { return ret; @@ -640,7 +651,7 @@ int UavcanNode::init(uavcan::NodeID node_id) } { - (void) param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed); + param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed); _esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0); } @@ -668,7 +679,8 @@ int UavcanNode::init(uavcan::NodeID node_id) return _node.start(); } -void UavcanNode::node_spin_once() +void +UavcanNode::node_spin_once() { const int spin_res = _node.spinOnce(); @@ -686,7 +698,8 @@ void 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 +UavcanNode::add_poll_fd(int fd) { int ret = _poll_fds_num; @@ -698,13 +711,13 @@ int UavcanNode::add_poll_fd(int fd) _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 &) +void +UavcanNode::handle_time_sync(const uavcan::TimerEvent &) { - /* * Check whether there are higher priority masters in the network. * If there are, we need to activate the local slave in order to sync with them. @@ -742,25 +755,19 @@ void UavcanNode::handle_time_sync(const uavcan::TimerEvent &) _time_sync_master.publish(); } - - -int UavcanNode::run() +int +UavcanNode::run() { - (void)pthread_mutex_lock(&_node_mutex); + pthread_mutex_lock(&_node_mutex); // XXX figure out the output count _output_count = 2; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - memset(&_outputs, 0, sizeof(_outputs)); - - /* - * Set up the time synchronization - */ + actuator_outputs_s outputs{}; + // Set up the time synchronization const int slave_init_res = _time_sync_slave.start(); if (slave_init_res < 0) { @@ -811,15 +818,16 @@ int UavcanNode::run() update_params(); - uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)}; - while (!_task_should_exit) { + perf_begin(_cycle_perf); + perf_count(_interval_perf); + // check for parameter updates - if (parameter_update_sub.updated()) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; - parameter_update_sub.copy(&pupdate); + _parameter_update_sub.copy(&pupdate); // update parameters from storage update_params(); @@ -878,30 +886,31 @@ int UavcanNode::run() } } - /* - see if we have any direct actuator updates - */ + // see if we have any direct actuator updates + actuator_direct_s actuator_direct{}; + if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && - orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &actuator_direct) == OK && !_test_in_progress) { - if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - _actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; + + if (actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { + actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } - memcpy(&_outputs.output[0], &_actuator_direct.values[0], - _actuator_direct.nvalues * sizeof(float)); - _outputs.noutputs = _actuator_direct.nvalues; + memcpy(&outputs.output[0], &actuator_direct.values[0], actuator_direct.nvalues * sizeof(float)); + outputs.noutputs = actuator_direct.nvalues; new_output = true; } // can we mix? if (_test_in_progress) { - memset(&_outputs, 0, sizeof(_outputs)); + + memset(&outputs, 0, sizeof(outputs)); if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) { - _outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; - _outputs.noutputs = _test_motor.motor_number + 1; + outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f; + outputs.noutputs = _test_motor.motor_number + 1; } new_output = true; @@ -916,7 +925,7 @@ int UavcanNode::run() _mixers->set_airmode(_airmode); // Do mixing - _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); new_output = true; } @@ -924,31 +933,25 @@ int UavcanNode::run() if (new_output) { // iterate actuators, checking for valid values - for (uint8_t i = 0; i < _outputs.noutputs; i++) { + for (uint8_t i = 0; i < outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(_outputs.output[i])) { + 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; + outputs.output[i] = -1.0f; } - // never go below min - if (_outputs.output[i] < -1.0f) { - _outputs.output[i] = -1.0f; - } - - // never go above max - if (_outputs.output[i] > 1.0f) { - _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(); + _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++) { @@ -956,19 +959,15 @@ int UavcanNode::run() const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; if (required && (timestamp_sample > 0)) { - perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample); + perf_set_elapsed(_control_latency_perf, outputs.timestamp - timestamp_sample); break; } } } - // Check motor test state - bool updated = false; - orb_check(_test_motor_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + 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); @@ -976,24 +975,25 @@ int UavcanNode::run() } // Check arming state - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + 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; + bool set_armed = (armed.armed && !armed.lockdown && !armed.manual_lockdown && !_test_in_progress); arm_actuators(set_armed); - if (_armed.soft_stop) { + 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); } (void)::close(busevent_fd); @@ -1024,7 +1024,7 @@ UavcanNode::teardown() } } - return (_armed_sub >= 0) ? orb_unsubscribe(_armed_sub) : 0; + return 0; } int @@ -1032,6 +1032,7 @@ UavcanNode::arm_actuators(bool arm) { _is_armed = arm; _esc_controller.arm_all_escs(arm); + return OK; } @@ -1186,6 +1187,8 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + printf("\n"); + // UAVCAN node perfcounters printf("UAVCAN node status:\n"); printf("\tInternal failures: %llu\n", _node.getInternalFailureCount()); @@ -1193,6 +1196,8 @@ UavcanNode::print_info() printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount()); printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + printf("\n"); + // CAN driver status for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { printf("CAN%u status:\n", unsigned(i + 1)); @@ -1206,28 +1211,14 @@ UavcanNode::print_info() printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); } + printf("\n"); + // ESC mixer status - printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + 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"); - if (_outputs.noutputs != 0) { - PX4_INFO("ESC output: "); - - for (uint8_t i = 0; i < _outputs.noutputs; i++) { - printf("%d ", (int)(_outputs.output[i] * 1000)); - } - - printf("\n"); - - // ESC status - int esc_sub = orb_subscribe(ORB_ID(esc_status)); - esc_status_s esc = {}; - orb_copy(ORB_ID(esc_status), esc_sub, &esc); - orb_unsubscribe(esc_sub); - - print_message(esc); - } + printf("\n"); // Sensor bridges for (const auto &br : _sensor_bridges) { @@ -1237,33 +1228,38 @@ UavcanNode::print_info() } // Printing all nodes that are online - std::printf("Online nodes (Node ID, Health, Mode):\n"); + printf("Online nodes (Node ID, Health, Mode):\n"); _node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) { - static constexpr const char *HEALTH[] = { - "OK", "WARN", "ERR", "CRIT" - }; - static constexpr const char *MODES[] = { - "OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN" - }; - std::printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]); + static constexpr const char *HEALTH[] = {"OK", "WARN", "ERR", "CRIT"}; + static constexpr const char *MODES[] = {"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"}; + printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]); }); + printf("\n"); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_control_latency_perf); + (void)pthread_mutex_unlock(&_node_mutex); } -void UavcanNode::shrink() +void +UavcanNode::shrink() { (void)pthread_mutex_lock(&_node_mutex); _pool_allocator.shrink(); (void)pthread_mutex_unlock(&_node_mutex); } -void UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command) +void +UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command) { (void)pthread_mutex_lock(&_node_mutex); _hardpoint_controller.set_command(hardpoint_id, command); (void)pthread_mutex_unlock(&_node_mutex); } + /* * App entry point */ diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index f84b85cf5b..7b25e6835e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -45,6 +45,12 @@ #include #include "uavcan_driver.hpp" +#include "uavcan_servers.hpp" +#include "allocator.hpp" +#include "actuators/esc.hpp" +#include "actuators/hardpoint.hpp" +#include "sensors/sensor_bridge.hpp" + #include #include #include @@ -53,21 +59,16 @@ #include #include -#include -#include +#include +#include -#include -#include +#include #include -#include +#include #include - -#include "actuators/esc.hpp" -#include "actuators/hardpoint.hpp" -#include "sensors/sensor_bridge.hpp" - -#include "uavcan_servers.hpp" -#include "allocator.hpp" +#include +#include +#include #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 @@ -131,10 +132,10 @@ public: void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command); - static UavcanNode *instance() { return _instance; } + static UavcanNode *instance() { return _instance; } static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); - void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} + void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} int list_params(int remote_node_id); int save_params(int remote_node_id); int set_param(int remote_node_id, const char *name, char *value); @@ -146,42 +147,38 @@ private: int init(uavcan::NodeID node_id); 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 print_params(uavcan::protocol::param::GetSet::Response &resp); int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); void update_params(); - 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; - 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 + void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; } + void free_setget_response(void) { _setget_response = nullptr; } - int _test_motor_sub = -1; ///< uORB subscription of the test_motor status - test_motor_s _test_motor = {}; - bool _test_in_progress = false; + 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}; + int _fw_server_status{-1}; - unsigned _output_count = 0; ///< number of actuators currently available + 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 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; UavcanHardpointController _hardpoint_controller; @@ -191,44 +188,52 @@ private: 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]; - 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; + MixerGroup *_mixers{nullptr}; + ITxQueueInjector *_tx_injector{nullptr}; - int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic - uint8_t _actuator_direct_poll_fd_num = 0; - actuator_direct_s _actuator_direct = {}; + 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}; - actuator_outputs_s _outputs = {}; + 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 _perf_control_latency; + int _actuator_direct_sub{-1}; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num{0}; - Mixer::Airmode _airmode = Mixer::Airmode::disabled; - float _thr_mdl_factor = 0.0f; + + 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]; + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {}; void handle_time_sync(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCallback; uavcan::TimerEventForwarder _master_timer; - bool _callback_success; - uavcan::protocol::param::GetSet::Response *_setget_response; + bool _callback_success{false}; + + uavcan::protocol::param::GetSet::Response *_setget_response{nullptr}; + typedef uavcan::MethodBinder &)> GetSetCallback; typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_setget(const uavcan::ServiceCallResult &result); void cb_opcode(const uavcan::ServiceCallResult &result); void cb_restart(const uavcan::ServiceCallResult &result); diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index c47c739fc1..6491afaccf 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -60,11 +60,9 @@ #include #include #include -#include #include - /** * @file uavcan_servers.cpp * @@ -78,8 +76,8 @@ * UavcanNode */ UavcanServers *UavcanServers::_instance; + UavcanServers::UavcanServers(uavcan::INode &main_node) : - _subnode_thread(-1), _vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), _subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()), _main_node(main_node), @@ -96,7 +94,6 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) : _enumeration_client(_subnode), _enumeration_getset_client(_subnode), _enumeration_save_client(_subnode) - { } @@ -109,17 +106,18 @@ UavcanServers::~UavcanServers() _main_node.getDispatcher().removeRxFrameListener(); } -int UavcanServers::stop() +int +UavcanServers::stop() { UavcanServers *server = instance(); if (server == nullptr) { - warnx("Already stopped"); + PX4_INFO("Already stopped"); return -1; } if (server->_subnode_thread) { - warnx("stopping fw srv thread..."); + PX4_INFO("stopping fw srv thread..."); server->_subnode_thread_should_exit = true; (void)pthread_join(server->_subnode_thread, NULL); } @@ -132,10 +130,11 @@ int UavcanServers::stop() return 0; } -int UavcanServers::start(uavcan::INode &main_node) +int +UavcanServers::start(uavcan::INode &main_node) { if (_instance != nullptr) { - warnx("Already started"); + PX4_INFO("Already started"); return -1; } @@ -145,14 +144,14 @@ int UavcanServers::start(uavcan::INode &main_node) _instance = new UavcanServers(main_node); if (_instance == nullptr) { - warnx("Out of memory"); + PX4_ERR("Out of memory"); return -2; } - int rv = _instance->init(); + int rv = _instance->init(); if (rv < 0) { - warnx("Node init failed: %d", rv); + PX4_ERR("Node init failed: %d", rv); delete _instance; _instance = nullptr; return rv; @@ -170,7 +169,7 @@ int UavcanServers::start(uavcan::INode &main_node) param.sched_priority = Priority; if (pthread_attr_setschedparam(&tattr, ¶m)) { - warnx("setting sched params failed"); + PX4_ERR("setting sched params failed"); } static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; @@ -179,7 +178,7 @@ int UavcanServers::start(uavcan::INode &main_node) if (rv != 0) { rv = -rv; - warnx("pthread_create() failed: %d", rv); + PX4_ERR("pthread_create() failed: %d", rv); delete _instance; _instance = nullptr; } @@ -187,7 +186,8 @@ int UavcanServers::start(uavcan::INode &main_node) return rv; } -int UavcanServers::init() +int +UavcanServers::init() { errno = 0; @@ -195,11 +195,10 @@ int UavcanServers::init() * Initialize the mutex. * giving it its path */ - int ret = Lock::init(_subnode_mutex); if (ret < 0) { - warnx("Lock init: %d", errno); + PX4_ERR("Lock init: %d", errno); return ret; } @@ -208,7 +207,6 @@ int UavcanServers::init() _subnode.setNodeID(_main_node.getNodeID()); _main_node.getDispatcher().installRxFrameListener(&_vdriver); - /* * Initialize the fw version checker. * giving it its path @@ -216,7 +214,7 @@ int UavcanServers::init() ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); if (ret < 0) { - warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno); + PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno); return ret; } @@ -225,7 +223,7 @@ int UavcanServers::init() ret = _fw_server.start(); if (ret < 0) { - warnx("BasicFileServer init: %d, errno: %d", ret, errno); + PX4_ERR("BasicFileServer init: %d, errno: %d", ret, errno); return ret; } @@ -234,7 +232,7 @@ int UavcanServers::init() ret = _storage_backend.init(UAVCAN_NODE_DB_PATH); if (ret < 0) { - warnx("FileStorageBackend init: %d, errno: %d", ret, errno); + PX4_ERR("FileStorageBackend init: %d, errno: %d", ret, errno); return ret; } @@ -243,7 +241,7 @@ int UavcanServers::init() ret = _tracer.init(UAVCAN_LOG_FILE); if (ret < 0) { - warnx("FileEventTracer init: %d, errno: %d", ret, errno); + PX4_ERR("FileEventTracer init: %d, errno: %d", ret, errno); return ret; } @@ -255,34 +253,32 @@ int UavcanServers::init() ret = _server_instance.init(hwver.unique_id); if (ret < 0) { - warnx("CentralizedServer init: %d", ret); + PX4_ERR("CentralizedServer init: %d", ret); return ret; } /* Start node info retriever to fetch node info from new nodes */ - ret = _node_info_retriever.start(); if (ret < 0) { - warnx("NodeInfoRetriever init: %d", ret); + PX4_ERR("NodeInfoRetriever init: %d", ret); return ret; } /* Start the fw version checker */ - ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); if (ret < 0) { - warnx("FirmwareUpdateTrigger init: %d", ret); + PX4_ERR("FirmwareUpdateTrigger init: %d", ret); return ret; } /* Start the Node */ - return 0; } -pthread_addr_t UavcanServers::run(pthread_addr_t) +pthread_addr_t +UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); @@ -296,9 +292,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) /* the subscribe call needs to happen in the same thread, * so not in the constructor */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + uORB::Subscription armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)}; /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); @@ -326,16 +322,13 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); if (spin_res < 0) { - warnx("node spin error %i", spin_res); + PX4_ERR("node spin error %i", spin_res); } // Check for parameter requests (get/set/list) - bool param_request_ready; - orb_check(param_request_sub, ¶m_request_ready); - - if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { - struct uavcan_parameter_request_s request; - orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); + if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + uavcan_parameter_request_s request{}; + param_request_sub.copy(&request); if (_param_counts[request.node_id]) { /* @@ -355,7 +348,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { - warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; @@ -388,7 +381,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { - warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; @@ -402,7 +395,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _param_list_node_id = request.node_id; _param_list_all_nodes = false; - warnx("UAVCAN command bridge: starting component-specific param list"); + PX4_INFO("UAVCAN command bridge: starting component-specific param list"); } } else if (request.node_id == MAV_COMP_ID_ALL) { @@ -416,7 +409,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _param_list_node_id = get_next_active_node_id(0); _param_list_all_nodes = true; - warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); + PX4_INFO("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); @@ -435,7 +428,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Handle parameter listing index/node ID advancement if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { if (_param_index >= _param_counts[_param_list_node_id]) { - warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); + PX4_INFO("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); // Reached the end of the current node's parameter set. _param_list_in_progress = false; @@ -457,7 +450,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Keep on listing. _param_index = 0; _param_list_in_progress = true; - warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); + PX4_INFO("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); } } } @@ -474,7 +467,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) if (call_res < 0) { _param_list_in_progress = false; - warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); + PX4_ERR("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; @@ -482,12 +475,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } // Check for ESC enumeration commands - bool cmd_ready; - orb_check(cmd_sub, &cmd_ready); - - if (cmd_ready && !_cmd_in_progress) { - struct vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + if (vcmd_sub.updated() && !_cmd_in_progress) { + vehicle_command_s cmd{}; + vcmd_sub.copy(&cmd); uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; @@ -496,7 +486,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int node_id = static_cast(cmd.param2 + 0.5f); int call_res; - warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); + PX4_INFO("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); switch (command_id) { case 0: @@ -512,7 +502,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) call_res = _enumeration_client.call(get_next_active_node_id(0), req); if (call_res < 0) { - warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); + PX4_ERR("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); beep(BeepFrequencyError); cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; @@ -524,7 +514,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } default: { - warnx("UAVCAN command bridge: unknown command ID %d", command_id); + PX4_ERR("UAVCAN command bridge: unknown command ID %d", command_id); cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED; break; } @@ -533,7 +523,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { int command_id = static_cast(cmd.param1 + 0.5f); - warnx("UAVCAN command bridge: received storage command ID %d", command_id); + PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id); switch (command_id) { case 1: { @@ -576,15 +566,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Shut down once armed // TODO (elsewhere): start up again once disarmed? - bool updated; - orb_check(armed_sub, &updated); - - if (updated) { - struct actuator_armed_s armed; - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + if (armed_sub.updated()) { + actuator_armed_s armed{}; + armed_sub.copy(&armed); if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) { - warnx("UAVCAN command bridge: system armed, exiting now."); + PX4_INFO("UAVCAN command bridge: system armed, exiting now."); break; } } @@ -592,11 +579,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _subnode_thread_should_exit = false; - warnx("exiting"); + PX4_INFO("exiting"); return (pthread_addr_t) 0; } -void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) { if (_count_in_progress) { /* @@ -622,14 +610,14 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult(); } - if (_param_response_pub == nullptr) { - _param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response); - - } else { - orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response); - } + _param_response_pub.publish(response); } else { - warnx("UAVCAN command bridge: GetSet error"); + PX4_ERR("UAVCAN command bridge: GetSet error"); } _param_in_progress = false; @@ -685,38 +668,41 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) { bool success = result.isSuccessful(); uint8_t node_id = result.getCallID().server_node_id.get(); @@ -725,23 +711,23 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) { bool success = result.isSuccessful(); uint8_t node_id = result.getCallID().server_node_id.get(); @@ -770,12 +757,13 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) { uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); if (!result.isSuccessful()) { - warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); + PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); } else if (result.getResponse().error) { - warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), - result.getResponse().error); + PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), + result.getResponse().error); } else { _esc_count++; - warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get()); } if (next_id < 128) { @@ -840,22 +832,23 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &msg) +void +UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure + &msg) { // Called whenever an ESC thinks it has received user input. - warnx("UAVCAN ESC enumeration: got indication"); + PX4_INFO("UAVCAN ESC enumeration: got indication"); if (!_esc_enumeration_active) { // Ignore any messages received when we're not expecting them @@ -867,33 +860,33 @@ void UavcanServers::cb_enumeration_indication(const for (; i < _esc_enumeration_index; i++) { if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) { - warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i); + PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i); return; } } uavcan::protocol::param::GetSet::Request req; - req.name = - msg.parameter_name; // 'esc_index' or something alike, the name is not standardized + req.name = msg.parameter_name; // 'esc_index' or something alike, the name is not standardized req.value.to() = i; int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req); if (call_res < 0) { - warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res); + PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res); } else { - warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i); + PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i); } } -void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) { if (!result.isSuccessful()) { - warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); } else { - warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); uavcan::protocol::param::GetSet::Response resp = result.getResponse(); uint8_t esc_index = (uint8_t)resp.value.to(); @@ -907,41 +900,43 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) +void +UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult &result) { const bool this_is_the_last_one = - _esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 || - _esc_enumeration_index >= _esc_count; + (_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) || + (_esc_enumeration_index >= _esc_count); if (!result.isSuccessful()) { - warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); beep(BeepFrequencyError); } else if (!result.getResponse().ok) { - warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get()); + PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get()); beep(BeepFrequencyError); } else { - warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication); } - warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count); + PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count); if (this_is_the_last_one) { _esc_enumeration_active = false; // Tell all ESCs to stop enumerating uavcan::protocol::enumeration::Begin::Request req; + // TODO: Incorrect implementation; the parameter name field should be left empty. // Leaving it as-is to avoid breaking compatibility with non-compliant nodes. req.parameter_name = "esc_index"; @@ -949,15 +944,16 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult maxlen) { - warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name); + PX4_WARN("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name); continue; } size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len; if (dstpath_dev_len > maxlen) { - warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name); + PX4_WARN("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name); continue; } @@ -1044,7 +1040,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); if (rv != 0) { - warnx("dev: couldn't create '%s'", dstpath); + PX4_ERR("dev: couldn't create '%s'", dstpath); continue; } } @@ -1056,7 +1052,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat DIR *const dev_dir = opendir(srcpath); if (!dev_dir) { - warnx("dev: couldn't open '%s'", srcpath); + PX4_ERR("dev: couldn't open '%s'", srcpath); continue; } @@ -1074,14 +1070,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len; if (srcpath_ver_len > maxlen) { - warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name); + PX4_ERR("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name); continue; } size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len; if (dstpath_ver_len > maxlen) { - warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name); + PX4_ERR("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name); continue; } @@ -1093,7 +1089,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); if (rv != 0) { - warnx("ver: couldn't create '%s'", dstpath); + PX4_ERR("ver: couldn't create '%s'", dstpath); continue; } } @@ -1107,7 +1103,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat DIR *const src_ver_dir = opendir(srcpath); if (!src_ver_dir) { - warnx("ver: couldn't open '%s'", srcpath); + PX4_ERR("ver: couldn't open '%s'", srcpath); continue; } @@ -1129,7 +1125,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat DIR *const dst_ver_dir = opendir(dstpath); if (!dst_ver_dir) { - warnx("unlink: couldn't open '%s'", dstpath); + PX4_ERR("unlink: couldn't open '%s'", dstpath); } else { struct dirent *fw_dirent = NULL; @@ -1153,14 +1149,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat if (dstpath_fw_len > maxlen) { // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator - warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); + PX4_ERR("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); } else { // File name starts with "_", delete it. dstpath[dstpath_ver_len] = '/'; memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1); unlink(dstpath); - warnx("unlink: removed '%s'", dstpath); + PX4_ERR("unlink: removed '%s'", dstpath); } } else { @@ -1178,10 +1174,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat size_t dstpath_fw_len = dstpath_ver_len + fw_len; if (srcpath_fw_len > maxlen) { - warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); + PX4_ERR("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); } else if (dstpath_fw_len > maxlen) { - warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name); + PX4_ERR("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name); } else { // All OK, make the paths and copy the file @@ -1194,10 +1190,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat rv = copyFw(dstpath, srcpath); if (rv != 0) { - warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv); + PX4_ERR("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv); } else { - warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath); + PX4_INFO("copy: '%s' -> '%s' succeeded", srcpath, dstpath); } } } @@ -1211,24 +1207,24 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat (void)closedir(romfs_dir); } -int UavcanServers::copyFw(const char *dst, const char *src) +int +UavcanServers::copyFw(const char *dst, const char *src) { int rv = 0; - int dfd, sfd; - uint8_t buffer[512]; + uint8_t buffer[512] {}; - dfd = open(dst, O_WRONLY | O_CREAT, 0666); + int dfd = open(dst, O_WRONLY | O_CREAT, 0666); if (dfd < 0) { - warnx("copyFw: couldn't open dst"); + PX4_ERR("copyFw: couldn't open dst"); return -errno; } - sfd = open(src, O_RDONLY, 0); + int sfd = open(src, O_RDONLY, 0); if (sfd < 0) { (void)close(dfd); - warnx("copyFw: couldn't open src"); + PX4_ERR("copyFw: couldn't open src"); return -errno; } @@ -1238,7 +1234,7 @@ int UavcanServers::copyFw(const char *dst, const char *src) size = read(sfd, buffer, sizeof(buffer)); if (size < 0) { - warnx("copyFw: couldn't read"); + PX4_ERR("copyFw: couldn't read"); rv = -errno; } else if (size > 0) { @@ -1251,7 +1247,7 @@ int UavcanServers::copyFw(const char *dst, const char *src) written = write(dfd, &buffer[total_written], remaining); if (written < 0) { - warnx("copyFw: couldn't write"); + PX4_ERR("copyFw: couldn't write"); rv = -errno; } else { diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 7465e546c3..a88f621dab 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -35,9 +35,11 @@ #include #include "uavcan_driver.hpp" -#include +#include #include +#include #include +#include #include #include @@ -110,9 +112,9 @@ public: bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); } private: - pthread_t _subnode_thread; - pthread_mutex_t _subnode_mutex; - volatile bool _subnode_thread_should_exit = false; + pthread_t _subnode_thread{-1}; + pthread_mutex_t _subnode_mutex{}; + volatile bool _subnode_thread_should_exit{false}; int init(); @@ -161,7 +163,7 @@ private: bool _cmd_in_progress = false; // uORB topic handle for MAVLink parameter responses - orb_advert_t _param_response_pub = nullptr; + uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_getset(const uavcan::ServiceCallResult &result); void cb_count(const uavcan::ServiceCallResult &result); void cb_opcode(const uavcan::ServiceCallResult &result); diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 849859a38f..60854158ef 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -140,12 +140,23 @@ public: DeviceBusType_SIMULATION = 4, }; - /** - * Return the bus ID the device is connected to. - * - * @return The bus ID + /* + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. */ - uint8_t get_device_bus() const { return _device_id.devid_s.bus; } + struct DeviceStructure { + DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; uint32_t get_device_id() const { return _device_id.devid; } @@ -154,7 +165,8 @@ public: * * @return The bus type */ - DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; } + DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; } + void set_device_bus_type(DeviceBusType bus_type) { _device_id.devid_s.bus_type = bus_type; } static const char *get_device_bus_string(DeviceBusType bus) { @@ -177,41 +189,29 @@ public: } } + /** + * Return the bus ID the device is connected to. + * + * @return The bus ID + */ + uint8_t get_device_bus() const { return _device_id.devid_s.bus; } + void set_device_bus(uint8_t bus) { _device_id.devid_s.bus = bus; } + /** * Return the bus address of the device. * * @return The bus address */ - uint8_t get_device_address() const { return _device_id.devid_s.address; } - - void set_device_address(int address) { _device_id.devid_s.address = address; } + uint8_t get_device_address() const { return _device_id.devid_s.address; } + void set_device_address(int address) { _device_id.devid_s.address = address; } /** - * Set the device type + * Return the device type * * @return The device type */ - void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; } - - virtual bool external() { return false; } - - /* - broken out device elements. The bitfields are used to keep - the overall value small enough to fit in a float accurately, - which makes it possible to transport over the MAVLink - parameter protocol without loss of information. - */ - struct DeviceStructure { - enum DeviceBusType bus_type : 3; - uint8_t bus: 5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; - - union DeviceId { - struct DeviceStructure devid_s; - uint32_t devid; - }; + uint8_t get_device_type() const { return _device_id.devid_s.devtype; } + void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; } /** * Print decoded device id string to a buffer. @@ -234,30 +234,34 @@ public: return num_written; } + virtual bool external() const { return false; } + protected: - union DeviceId _device_id {}; /**< device identifier information */ + union DeviceId _device_id {}; /**< device identifier information */ const char *_name{nullptr}; /**< driver name */ bool _debug_enabled{false}; /**< if true, debug messages are printed */ explicit Device(const char *name) : _name(name) { - /* setup a default device ID. When bus_type is UNKNOWN the - other fields are invalid */ - _device_id.devid = 0; - _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; - _device_id.devid_s.bus = 0; - _device_id.devid_s.address = 0; - _device_id.devid_s.devtype = 0; + set_device_bus_type(DeviceBusType_UNKNOWN); + } + + Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0) + : _name(name) + { + set_device_bus_type(bus_type); + set_device_bus(bus); + set_device_address(address); + set_device_type(devtype); } Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0) { - _device_id.devid = 0; - _device_id.devid_s.bus_type = bus_type; - _device_id.devid_s.bus = bus; - _device_id.devid_s.address = address; - _device_id.devid_s.devtype = devtype; + set_device_bus_type(bus_type); + set_device_bus(bus); + set_device_address(address); + set_device_type(devtype); } }; diff --git a/src/lib/drivers/device/nuttx/I2C.hpp b/src/lib/drivers/device/nuttx/I2C.hpp index e5754be56d..b098ea8847 100644 --- a/src/lib/drivers/device/nuttx/I2C.hpp +++ b/src/lib/drivers/device/nuttx/I2C.hpp @@ -65,7 +65,7 @@ public: I2C(I2C &&) = delete; I2C &operator=(I2C &&) = delete; - virtual int init(); + virtual int init() override; static int set_bus_clock(unsigned bus, unsigned clock_hz); @@ -109,7 +109,7 @@ protected: */ int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } + virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: uint32_t _frequency{0}; diff --git a/src/lib/drivers/device/nuttx/SPI.hpp b/src/lib/drivers/device/nuttx/SPI.hpp index 44118df5b2..0bab23b312 100644 --- a/src/lib/drivers/device/nuttx/SPI.hpp +++ b/src/lib/drivers/device/nuttx/SPI.hpp @@ -80,7 +80,7 @@ protected: LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ }; - virtual int init(); + virtual int init() override; /** * Check for the presence of the device on the bus. @@ -164,7 +164,7 @@ protected: int _transferhword(uint16_t *send, uint16_t *recv, unsigned len); - bool external() { return px4_spi_bus_external(get_device_bus()); } + virtual bool external() const override { return px4_spi_bus_external(get_device_bus()); } }; diff --git a/src/lib/drivers/device/posix/I2C.hpp b/src/lib/drivers/device/posix/I2C.hpp index c463e871da..ab9e08ecfc 100644 --- a/src/lib/drivers/device/posix/I2C.hpp +++ b/src/lib/drivers/device/posix/I2C.hpp @@ -66,7 +66,7 @@ public: I2C(I2C &&) = delete; I2C &operator=(I2C &&) = delete; - virtual int init(); + virtual int init() override; protected: /** @@ -106,7 +106,7 @@ protected: */ int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } + virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: int _fd{-1}; diff --git a/src/lib/drivers/device/posix/SPI.hpp b/src/lib/drivers/device/posix/SPI.hpp index 8950cc784d..ce7e76555b 100644 --- a/src/lib/drivers/device/posix/SPI.hpp +++ b/src/lib/drivers/device/posix/SPI.hpp @@ -95,7 +95,7 @@ protected: LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ }; - virtual int init(); + virtual int init() override; /** * Check for the presence of the device on the bus. @@ -179,7 +179,7 @@ protected: int _transferhword(uint16_t *send, uint16_t *recv, unsigned len); - bool external() { return px4_spi_bus_external(get_device_bus()); } + virtual bool external() const override { return px4_spi_bus_external(get_device_bus()); } }; diff --git a/src/lib/parameters/parameters_injected.xml b/src/lib/parameters/parameters_injected.xml index 2f164ee52b..9347cf91ac 100644 --- a/src/lib/parameters/parameters_injected.xml +++ b/src/lib/parameters/parameters_injected.xml @@ -92,7 +92,7 @@ 0 4000 - + READ ONLY: Motor inductance in henries. READ ONLY: Motor inductance in henries. This is measured on start-up. henries @@ -135,4 +135,66 @@ 0 - \ No newline at end of file + + + device health warning + Set the device health to Warning if the dimensionality of + the GNSS solution is less than this value. 3 for the full (3D) + solution, 2 for planar (2D) solution, 1 for time-only solution, + 0 disables the feature. + + 0 + 3 + + disables the feature + time-only solution + planar (2D) solution + full (3D) solution + + + + + Set the device health to Warning if the number of satellites + used in the GNSS solution is below this threshold. Zero + disables the feature + + + + GNSS dynamic model + Dynamic model used in the GNSS positioning engine. 0 – + Automotive, 1 – Sea, 2 – Airborne. + + 0 + 2 + + Automotive + Sea + Airborne + + + + Broadcast old GNSS fix message + Broadcast the old (deprecated) GNSS fix message + uavcan.equipment.gnss.Fix alongside the new alternative + uavcan.equipment.gnss.Fix2. It is recommended to + disable this feature to reduce the CAN bus traffic. + + 0 + 1 + + Fix2 + Fix and Fix2 + + + + + Set the device health to Warning if the number of satellites + used in the GNSS solution is below this threshold. Zero + disables the feature + + microseconds + 0 + 1000000 + + +