From cf43d07f708fb13f41b69ec9770231e2983e5fd3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 27 Jan 2021 20:57:23 -0500 Subject: [PATCH] uavcannode: refactor into separate publishers and subscribers --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/drivers/uavcannode/CMakeLists.txt | 1 - .../uavcannode/Publishers/BatteryInfo.hpp | 102 ++++++ .../uavcannode/Publishers/FlowMeasurement.hpp | 89 +++++ .../uavcannode/Publishers/GnssFix2.hpp | 107 ++++++ .../Publishers/MagneticFieldStrength2.hpp | 86 +++++ .../Publishers/RangeSensorMeasurement.hpp | 121 +++++++ .../uavcannode/Publishers/RawAirData.hpp | 90 ++++++ .../uavcannode/Publishers/SafetyButton.hpp | 83 +++++ .../uavcannode/Publishers/StaticPressure.hpp | 83 +++++ .../Publishers/StaticTemperature.hpp | 87 +++++ .../Publishers/UavcanPublisherBase.hpp | 67 ++++ .../uavcannode/Subscribers/BeepCommand.hpp | 94 ++++++ .../uavcannode/Subscribers/LightsCommand.hpp | 149 +++++++++ .../UavcanSubscriberBase.hpp} | 35 +- src/drivers/uavcannode/UavcanNode.cpp | 305 ++++-------------- src/drivers/uavcannode/UavcanNode.hpp | 75 ++--- .../uavcannode/UavcanNodeParamManager.cpp | 5 + .../uavcannode/UavcanNodeParamManager.hpp | 4 + .../uavcannode/indication_controller.cpp | 151 --------- 20 files changed, 1286 insertions(+), 450 deletions(-) create mode 100644 src/drivers/uavcannode/Publishers/BatteryInfo.hpp create mode 100644 src/drivers/uavcannode/Publishers/FlowMeasurement.hpp create mode 100644 src/drivers/uavcannode/Publishers/GnssFix2.hpp create mode 100644 src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp create mode 100644 src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp create mode 100644 src/drivers/uavcannode/Publishers/RawAirData.hpp create mode 100644 src/drivers/uavcannode/Publishers/SafetyButton.hpp create mode 100644 src/drivers/uavcannode/Publishers/StaticPressure.hpp create mode 100644 src/drivers/uavcannode/Publishers/StaticTemperature.hpp create mode 100644 src/drivers/uavcannode/Publishers/UavcanPublisherBase.hpp create mode 100644 src/drivers/uavcannode/Subscribers/BeepCommand.hpp create mode 100644 src/drivers/uavcannode/Subscribers/LightsCommand.hpp rename src/drivers/uavcannode/{indication_controller.hpp => Subscribers/UavcanSubscriberBase.hpp} (68%) delete mode 100644 src/drivers/uavcannode/indication_controller.cpp 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 397490256b..a2e81faf4e 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -75,7 +75,7 @@ static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; -static constexpr wq_config_t uavcan{"wq:uavcan", 3680, -19}; +static constexpr wq_config_t uavcan{"wq:uavcan", 2176, -19}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 8a8d7fc9b4..f41849b861 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -129,7 +129,6 @@ px4_add_module( SRCS allocator.hpp uavcan_driver.hpp - indication_controller.cpp UavcanNode.cpp UavcanNode.hpp UavcanNodeParamManager.hpp diff --git a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp new file mode 100644 index 0000000000..ca0a672306 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class BatteryInfo : + public UavcanPublisherBase, + private uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + BatteryInfo(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(battery_status)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::power::BatteryInfo::getDataTypeFullName(), + uavcan::equipment::power::BatteryInfo::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // battery_status -> uavcan::equipment::power::BatteryInfo + battery_status_s battery; + + if (uORB::SubscriptionCallbackWorkItem::update(&battery)) { + uavcan::equipment::power::BatteryInfo battery_info{}; + battery_info.voltage = battery.voltage_v; + battery_info.current = fabs(battery.current_a); + battery_info.temperature = battery.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // convert from C to K + battery_info.full_charge_capacity_wh = battery.capacity; + battery_info.remaining_capacity_wh = battery.remaining * battery.capacity; + battery_info.state_of_charge_pct = battery.remaining * 100; + battery_info.state_of_charge_pct_stdev = battery.max_error; + battery_info.model_instance_id = 0; // TODO: what goes here? + battery_info.model_name = "ARK BMS Rev 0.2"; + battery_info.battery_id = battery.serial_number; + battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL + battery_info.state_of_health_pct = battery.state_of_health; + + if (battery.current_a > 0.0f) { + battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_CHARGING; + + } else { + battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; + } + + uavcan::Publisher::broadcast(battery_info); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcan diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp new file mode 100644 index 0000000000..4cf060eebf --- /dev/null +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class FlowMeasurement : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + com::hex::equipment::flow::Measurement::getDataTypeFullName(), + com::hex::equipment::flow::Measurement::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // optical_flow -> com::hex::equipment::flow::Measurement + optical_flow_s optical_flow; + + if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) { + com::hex::equipment::flow::Measurement measurement{}; + measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s + measurement.rate_gyro_integral[0] = optical_flow.gyro_x_rate_integral; + measurement.rate_gyro_integral[1] = optical_flow.gyro_y_rate_integral; + measurement.flow_integral[0] = optical_flow.pixel_flow_x_integral; + measurement.flow_integral[1] = optical_flow.pixel_flow_y_integral; + measurement.quality = optical_flow.quality; + + uavcan::Publisher::broadcast(measurement); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp new file mode 100644 index 0000000000..c6b5fdd8ad --- /dev/null +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class GnssFix2 : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + GnssFix2(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::gnss::Fix2::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::gnss::Fix2::getDataTypeFullName(), + id()); + } + } + + void BroadcastAnyUpdates() override + { + // sensor_gps -> uavcan::equipment::gnss::Fix2 + sensor_gps_s gps; + + if (uORB::SubscriptionCallbackWorkItem::update(&gps)) { + uavcan::equipment::gnss::Fix2 fix2{}; + + fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC; + fix2.gnss_timestamp.usec = gps.time_utc_usec; + fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10; + fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10; + fix2.height_msl_mm = gps.alt; + fix2.height_ellipsoid_mm = gps.alt_ellipsoid; + fix2.status = gps.fix_type; + fix2.ned_velocity[0] = gps.vel_n_m_s; + fix2.ned_velocity[1] = gps.vel_e_m_s; + fix2.ned_velocity[2] = gps.vel_d_m_s; + fix2.pdop = gps.hdop > gps.vdop ? gps.hdop : + gps.vdop; // Use pdop for both hdop and vdop since uavcan v0 spec does not support them + fix2.sats_used = gps.satellites_used; + + // Diagonal matrix + // position variances -- Xx, Yy, Zz + fix2.covariance.push_back(gps.eph); + fix2.covariance.push_back(gps.eph); + fix2.covariance.push_back(gps.eph); + // velocity variance -- Vxx, Vyy, Vzz + fix2.covariance.push_back(gps.s_variance_m_s); + fix2.covariance.push_back(gps.s_variance_m_s); + fix2.covariance.push_back(gps.s_variance_m_s); + + uavcan::Publisher::broadcast(fix2); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp new file mode 100644 index 0000000000..4e421ae3fc --- /dev/null +++ b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class MagneticFieldStrength2 : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + MagneticFieldStrength2(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::ahrs::MagneticFieldStrength2::getDataTypeFullName(), + uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2 + sensor_mag_s mag; + + if (uORB::SubscriptionCallbackWorkItem::update(&mag)) { + uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{}; + magnetic_field.sensor_id = mag.device_id; + magnetic_field.magnetic_field_ga[0] = mag.x; + magnetic_field.magnetic_field_ga[1] = mag.y; + magnetic_field.magnetic_field_ga[2] = mag.z; + uavcan::Publisher::broadcast(magnetic_field); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp new file mode 100644 index 0000000000..06e0fe451f --- /dev/null +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class RangeSensorMeasurement : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + RangeSensorMeasurement(px4::WorkItem *work_item, uavcan::INode &node, uint8_t instance = 0) : + UavcanPublisherBase(uavcan::equipment::range_sensor::Measurement::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(distance_sensor), instance), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::range_sensor::Measurement::getDataTypeFullName(), + uavcan::equipment::range_sensor::Measurement::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // distance_sensor[] -> uavcan::equipment::range_sensor::Measurement + distance_sensor_s dist; + + if (uORB::SubscriptionCallbackWorkItem::update(&dist)) { + uavcan::equipment::range_sensor::Measurement range_sensor{}; + + range_sensor.sensor_id = get_instance(); + range_sensor.range = dist.current_distance; + range_sensor.field_of_view = dist.h_fov; + + // sensor type + switch (dist.type) { + case distance_sensor_s::MAV_DISTANCE_SENSOR_LASER: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED: + default: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED; + break; + } + + // reading_type + if (dist.current_distance >= dist.max_distance) { + range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; + + } else if (dist.current_distance <= dist.min_distance) { + range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; + + } else if (dist.signal_quality != 0) { + range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE; + + } else { + range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_UNDEFINED; + } + + uavcan::Publisher::broadcast(range_sensor); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/RawAirData.hpp b/src/drivers/uavcannode/Publishers/RawAirData.hpp new file mode 100644 index 0000000000..98035e3a46 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/RawAirData.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class RawAirData : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + RawAirData(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::air_data::RawAirData::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(differential_pressure)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::air_data::RawAirData::getDataTypeFullName(), + uavcan::equipment::air_data::RawAirData::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // differential_pressure -> uavcan::equipment::air_data::RawAirData + differential_pressure_s diff_press; + + if (uORB::SubscriptionCallbackWorkItem::update(&diff_press)) { + uavcan::equipment::air_data::RawAirData raw_air_data{}; + + // raw_air_data.static_pressure = + raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa; + // raw_air_data.static_pressure_sensor_temperature = + raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; + raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; + // raw_air_data.pitot_temperature + // raw_air_data.covariance + uavcan::Publisher::broadcast(raw_air_data); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/SafetyButton.hpp b/src/drivers/uavcannode/Publishers/SafetyButton.hpp new file mode 100644 index 0000000000..cab88c1294 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/SafetyButton.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class SafetyButton : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + SafetyButton(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(standard::indication::Button::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + standard::indication::Button::getDataTypeFullName(), + standard::indication::Button::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // safety -> standard::indication::button + safety_s safety; + + if (uORB::SubscriptionCallbackWorkItem::update(&safety)) { + if (safety.safety_switch_available) { + standard::indication::Button Button{}; + Button.button = standard::indication::Button::BUTTON_SAFETY; + Button.press_time = safety.safety_off ? UINT8_MAX : 0; + uavcan::Publisher::broadcast(Button); + } + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/StaticPressure.hpp b/src/drivers/uavcannode/Publishers/StaticPressure.hpp new file mode 100644 index 0000000000..baab9c123e --- /dev/null +++ b/src/drivers/uavcannode/Publishers/StaticPressure.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class StaticPressure : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + StaticPressure(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::air_data::StaticPressure::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::air_data::StaticPressure::getDataTypeFullName(), + uavcan::equipment::air_data::StaticPressure::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // sensor_baro -> uavcan::equipment::air_data::StaticPressure + sensor_baro_s baro; + + if (uORB::SubscriptionCallbackWorkItem::update(&baro)) { + uavcan::equipment::air_data::StaticPressure static_pressure{}; + static_pressure.static_pressure = baro.pressure * 100; // millibar -> pascals + uavcan::Publisher::broadcast(static_pressure); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/StaticTemperature.hpp b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp new file mode 100644 index 0000000000..5216488e98 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class StaticTemperature : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + StaticTemperature(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::air_data::StaticTemperature::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)), + uavcan::Publisher(node) + {} + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::air_data::StaticTemperature::getDataTypeFullName(), + uavcan::equipment::air_data::StaticTemperature::DefaultDataTypeID); + } + } + + void BroadcastAnyUpdates() override + { + // sensor_baro -> uavcan::equipment::air_data::StaticTemperature + sensor_baro_s baro; + + if ((hrt_elapsed_time(&_last_static_temperature_publish) > 1_s) && uORB::SubscriptionCallbackWorkItem::update(&baro)) { + uavcan::equipment::air_data::StaticTemperature static_temperature{}; + static_temperature.static_temperature = baro.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; + uavcan::Publisher::broadcast(static_temperature); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + + _last_static_temperature_publish = hrt_absolute_time(); + } + } +private: + hrt_abstime _last_static_temperature_publish{0}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Publishers/UavcanPublisherBase.hpp b/src/drivers/uavcannode/Publishers/UavcanPublisherBase.hpp new file mode 100644 index 0000000000..d5e9039732 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/UavcanPublisherBase.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +namespace uavcannode +{ + +class UavcanPublisherBase : public IntrusiveSortedListNode +{ +public: + UavcanPublisherBase() = delete; + explicit UavcanPublisherBase(uint16_t id) : _id(id) {} + + virtual ~UavcanPublisherBase() = default; + + /** + * Prints current status in a human readable format to stdout. + */ + virtual void PrintInfo() = 0; + + virtual void BroadcastAnyUpdates() = 0; + + // sorted numerically by ID + bool operator<=(UavcanPublisherBase &rhs) { return id() <= rhs.id(); } + + uint16_t id() const { return _id; } + +private: + uint16_t _id{0}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Subscribers/BeepCommand.hpp b/src/drivers/uavcannode/Subscribers/BeepCommand.hpp new file mode 100644 index 0000000000..f5a0f578bf --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/BeepCommand.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class BeepCommand; + +typedef uavcan::MethodBinder&)> + BeepcommandBinder; + +class BeepCommand : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + BeepCommand(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::indication::BeepCommand::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(BeepcommandBinder(this, &BeepCommand::callback)) < 0) { + PX4_ERR("uavcan::equipment::indication::BeepCommand subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::indication::BeepCommand::getDataTypeFullName(), + uavcan::equipment::indication::BeepCommand::DefaultDataTypeID, + _tune_control_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + tune_control_s tune_control{}; + tune_control.tune_id = 0; + tune_control.frequency = (uint16_t)msg.frequency; + tune_control.duration = uavcan::uint32_t(1000000 * msg.duration); + tune_control.volume = 0xff; + tune_control.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune_control); + } + + uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/Subscribers/LightsCommand.hpp b/src/drivers/uavcannode/Subscribers/LightsCommand.hpp new file mode 100644 index 0000000000..b42f3d6d99 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/LightsCommand.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "UavcanSubscriberBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class LightsCommand; + +typedef uavcan::MethodBinder&)> + LightscommandBinder; + +class LightsCommand : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + LightsCommand(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::indication::LightsCommand::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(LightscommandBinder(this, &LightsCommand::callback)) < 0) { + PX4_ERR("uavcan::equipment::indication::LightsCommand subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::indication::LightsCommand::getDataTypeFullName(), + uavcan::equipment::indication::LightsCommand::DefaultDataTypeID, + _led_control_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + uavcan::uint32_t red = 0; + uavcan::uint32_t green = 0; + uavcan::uint32_t blue = 0; + + for (auto &cmd : msg.commands) { + if (cmd.light_id == _self_light_index) { + using uavcan::equipment::indication::RGB565; + + red = uavcan::uint32_t(float(cmd.color.red) * + (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F); + + green = uavcan::uint32_t(float(cmd.color.green) * + (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F); + + blue = uavcan::uint32_t(float(cmd.color.blue) * + (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F); + + red = uavcan::min(red, 0xFFU); + green = uavcan::min(green, 0xFFU); + blue = uavcan::min(blue, 0xFFU); + + led_control_s led_control{}; + led_control.num_blinks = 0; + led_control.priority = led_control_s::MAX_PRIORITY; + led_control.mode = led_control_s::MODE_OFF; + led_control.led_mask = 0xff; + led_control.color = led_control_s::COLOR_OFF; + + if (red != 0 && blue == 0 && green == 0) { + led_control.color = led_control_s::COLOR_RED; + + } else if (red == 0 && blue != 0 && green == 0) { + led_control.color = led_control_s::COLOR_BLUE; + + } else if (red == 0 && blue == 0 && green != 0) { + led_control.color = led_control_s::COLOR_GREEN; + + } else if (red != 0 && blue == 0 && green != 0) { + led_control.color = led_control_s::COLOR_YELLOW; + + } else if (red != 0 && blue != 0 && green == 0) { + led_control.color = led_control_s::COLOR_PURPLE; + + } else if (red != 0 && blue == 0 && green != 0 && red > green) { + led_control.color = led_control_s::COLOR_AMBER; + + } else if (red == 0 && blue != 0 && green != 0) { + led_control.color = led_control_s::COLOR_CYAN; + + } else if (red != 0 && blue != 0 && green != 0) { + led_control.color = led_control_s::COLOR_WHITE; + } + + if (led_control.color != led_control_s::COLOR_OFF) { + led_control.mode = led_control_s::MODE_ON; + } + + led_control.timestamp = hrt_absolute_time(); + _led_control_pub.publish(led_control); + } + } + } + + uORB::Publication _led_control_pub{ORB_ID(led_control)}; + unsigned _self_light_index{0}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/indication_controller.hpp b/src/drivers/uavcannode/Subscribers/UavcanSubscriberBase.hpp similarity index 68% rename from src/drivers/uavcannode/indication_controller.hpp rename to src/drivers/uavcannode/Subscribers/UavcanSubscriberBase.hpp index 128e5f2830..c838b96738 100644 --- a/src/drivers/uavcannode/indication_controller.hpp +++ b/src/drivers/uavcannode/Subscribers/UavcanSubscriberBase.hpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * Copyright (c) 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,5 +33,35 @@ #pragma once +#include +#include -int init_indication_controller(uavcan::INode &node); +#include + +namespace uavcannode +{ + +class UavcanSubscriberBase : public IntrusiveSortedListNode +{ +public: + UavcanSubscriberBase() = delete; + explicit UavcanSubscriberBase(uint16_t id) : _id(id) {} + + virtual ~UavcanSubscriberBase() = default; + + virtual bool init() = 0; + + /** + * Prints current status in a human readable format to stdout. + */ + virtual void PrintInfo() const {}; + + // sorted numerically by ID + bool operator<=(UavcanSubscriberBase &rhs) { return id() <= rhs.id(); } + + uint16_t id() const { return _id; } + +private: + uint16_t _id{0}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 2f444646ab..a45392d364 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -32,15 +32,30 @@ ****************************************************************************/ #include "UavcanNode.hpp" -#include "indication_controller.hpp" #include "boot_app_shared.h" #include #include +#include "Publishers/BatteryInfo.hpp" +#include "Publishers/FlowMeasurement.hpp" +#include "Publishers/GnssFix2.hpp" +#include "Publishers/MagneticFieldStrength2.hpp" +#include "Publishers/RangeSensorMeasurement.hpp" +#include "Publishers/RawAirData.hpp" +#include "Publishers/SafetyButton.hpp" +#include "Publishers/StaticPressure.hpp" +#include "Publishers/StaticTemperature.hpp" + +#include "Subscribers/BeepCommand.hpp" +#include "Subscribers/LightsCommand.hpp" + using namespace time_literals; +namespace uavcannode +{ + /** * @file uavcan_main.cpp * @@ -57,7 +72,6 @@ using namespace time_literals; * uavcan bootloader has the ability to validate the * image crc, size etc of this application */ - boot_app_shared_section app_descriptor_t AppDescriptor = { .signature = {APP_DESCRIPTOR_SIGNATURE}, .image_crc = 0, @@ -75,18 +89,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _node(can_driver, system_clock, _pool_allocator), _time_sync_slave(_node), _fw_update_listner(_node), - _ahrs_magnetic_field_strength2_publisher(_node), - _gnss_fix2_publisher(_node), - _power_battery_info_publisher(_node), - _air_data_static_pressure_publisher(_node), - _air_data_static_temperature_publisher(_node), - _raw_air_data_publisher(_node), - _range_sensor_measurement(_node), - _flow_measurement_publisher(_node), - _indication_button_publisher(_node), _param_server(_node), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), - _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")), _reset_timer(_node) { int res = pthread_mutex_init(&_node_mutex, nullptr); @@ -116,14 +119,15 @@ UavcanNode::~UavcanNode() } while (_instance); } + _publisher_list.clear(); + _subscriber_list.clear(); + perf_free(_cycle_perf); perf_free(_interval_perf); } int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) { - int rv = -1; - if (UavcanNode::instance()) { hwver.major = HW_VERSION_MAJOR; @@ -132,10 +136,10 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) mfguid_t mfgid = {}; board_get_mfguid(mfgid); uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin()); - rv = 0; + return 0; } - return rv; + return -1; } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -261,13 +265,29 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events fill_node_info(); - const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this, - &UavcanNode::cb_beginfirmware_update)); - - if (srv_start_res < 0) { + if (_fw_update_listner.start(BeginFirmwareUpdateCallBack(this, &UavcanNode::cb_beginfirmware_update)) < 0) { + PX4_ERR("firmware update listener start failed"); return PX4_ERROR; } + // TODO: make runtime (and build time?) configurable + _publisher_list.add(new BatteryInfo(this, _node)); + _publisher_list.add(new FlowMeasurement(this, _node)); + _publisher_list.add(new GnssFix2(this, _node)); + _publisher_list.add(new MagneticFieldStrength2(this, _node)); + _publisher_list.add(new RangeSensorMeasurement(this, _node)); + _publisher_list.add(new RawAirData(this, _node)); + _publisher_list.add(new SafetyButton(this, _node)); + _publisher_list.add(new StaticPressure(this, _node)); + _publisher_list.add(new StaticTemperature(this, _node)); + + _subscriber_list.add(new BeepCommand(_node)); + _subscriber_list.add(new LightsCommand(_node)); + + for (auto &subscriber : _subscriber_list) { + subscriber->init(); + } + bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); return _node.start(); @@ -294,8 +314,6 @@ void UavcanNode::Run() get_node().setRestartRequestHandler(&restart_request_handler); _param_server.start(&_param_manager); - init_indication_controller(get_node()); - // Set up the time synchronization const int slave_init_res = _time_sync_slave.start(); @@ -306,18 +324,6 @@ void UavcanNode::Run() _node.setModeOperational(); - _diff_pressure_sub.registerCallback(); - - for (auto &dist : _distance_sensor_sub) { - dist.registerCallback(); - } - - _optical_flow_sub.registerCallback(); - _sensor_baro_sub.registerCallback(); - _sensor_gps_sub.registerCallback(); - _sensor_mag_sub.registerCallback(); - _sensor_gps_sub.registerCallback(); - _initialized = true; } @@ -333,208 +339,13 @@ void UavcanNode::Run() // update parameters from storage } - const int spin_res = _node.spin(uavcan::MonotonicTime()); + _node.spinOnce(); - if (spin_res < 0) { - PX4_ERR("node spin error %i", spin_res); + for (auto &publisher : _publisher_list) { + publisher->BroadcastAnyUpdates(); } - // battery_status -> uavcan::equipment::power::BatteryInfo - if (_battery_status_sub.updated()) { - battery_status_s battery; - - if (_battery_status_sub.copy(&battery)) { - uavcan::equipment::power::BatteryInfo battery_info{}; - battery_info.voltage = battery.voltage_v; - battery_info.current = fabs(battery.current_a); - battery_info.temperature = battery.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // convert from C to K - battery_info.full_charge_capacity_wh = battery.capacity; - battery_info.remaining_capacity_wh = battery.remaining * battery.capacity; - battery_info.state_of_charge_pct = battery.remaining * 100; - battery_info.state_of_charge_pct_stdev = battery.max_error; - battery_info.model_instance_id = 0; // TODO: what goes here? - battery_info.model_name = "ARK BMS Rev 0.2"; - battery_info.battery_id = battery.serial_number; - battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL - battery_info.state_of_health_pct = battery.state_of_health; - - if (battery.current_a > 0.0f) { - battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_CHARGING; - - } else { - battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; - } - - _power_battery_info_publisher.broadcast(battery_info); - } - } - - // differential_pressure -> uavcan::equipment::air_data::RawAirData - if (_diff_pressure_sub.updated()) { - differential_pressure_s diff_press; - - if (_diff_pressure_sub.copy(&diff_press)) { - - uavcan::equipment::air_data::RawAirData raw_air_data{}; - - // raw_air_data.static_pressure = - raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa; - // raw_air_data.static_pressure_sensor_temperature = - raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; - raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; - // raw_air_data.pitot_temperature - // raw_air_data.covariance - _raw_air_data_publisher.broadcast(raw_air_data); - } - } - - // distance_sensor[] -> uavcan::equipment::range_sensor::Measurement - for (int i = 0; i < MAX_INSTANCES; i++) { - distance_sensor_s dist; - - if (_distance_sensor_sub[i].update(&dist)) { - uavcan::equipment::range_sensor::Measurement range_sensor{}; - - range_sensor.sensor_id = i; - range_sensor.range = dist.current_distance; - range_sensor.field_of_view = dist.h_fov; - - // sensor type - switch (dist.type) { - case distance_sensor_s::MAV_DISTANCE_SENSOR_LASER: - range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; - break; - - case distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND: - range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR; - break; - - case distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR: - range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR; - break; - - case distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED: - default: - range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED; - break; - } - - // reading_type - if (dist.current_distance >= dist.max_distance) { - range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - - } else if (dist.current_distance <= dist.min_distance) { - range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; - - } else if (dist.signal_quality != 0) { - range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE; - - } else { - range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_UNDEFINED; - } - - _range_sensor_measurement.broadcast(range_sensor); - } - } - - // sensor_baro -> uavcan::equipment::air_data::StaticTemperature - if (_sensor_baro_sub.updated()) { - sensor_baro_s baro; - - if (_sensor_baro_sub.copy(&baro)) { - uavcan::equipment::air_data::StaticPressure static_pressure{}; - static_pressure.static_pressure = baro.pressure * 100; // millibar -> pascals - _air_data_static_pressure_publisher.broadcast(static_pressure); - - if (hrt_elapsed_time(&_last_static_temperature_publish) > 1_s) { - uavcan::equipment::air_data::StaticTemperature static_temperature{}; - static_temperature.static_temperature = baro.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; - _air_data_static_temperature_publisher.broadcast(static_temperature); - _last_static_temperature_publish = hrt_absolute_time(); - } - } - } - - // sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2 - if (_sensor_mag_sub.updated()) { - sensor_mag_s mag; - - if (_sensor_mag_sub.copy(&mag)) { - uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{}; - magnetic_field.sensor_id = mag.device_id; - magnetic_field.magnetic_field_ga[0] = mag.x; - magnetic_field.magnetic_field_ga[1] = mag.y; - magnetic_field.magnetic_field_ga[2] = mag.z; - _ahrs_magnetic_field_strength2_publisher.broadcast(magnetic_field); - } - } - - // sensor_gps -> uavcan::equipment::gnss::Fix2 - if (_sensor_gps_sub.updated()) { - sensor_gps_s gps; - - if (_sensor_gps_sub.copy(&gps)) { - uavcan::equipment::gnss::Fix2 fix2{}; - - fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC; - fix2.gnss_timestamp.usec = gps.time_utc_usec; - fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10; - fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10; - fix2.height_msl_mm = gps.alt; - fix2.height_ellipsoid_mm = gps.alt_ellipsoid; - fix2.status = gps.fix_type; - fix2.ned_velocity[0] = gps.vel_n_m_s; - fix2.ned_velocity[1] = gps.vel_e_m_s; - fix2.ned_velocity[2] = gps.vel_d_m_s; - fix2.pdop = gps.hdop > gps.vdop ? gps.hdop : - gps.vdop; // Use pdop for both hdop and vdop since uavcan v0 spec does not support them - fix2.sats_used = gps.satellites_used; - - // Diagonal matrix - // position variances -- Xx, Yy, Zz - fix2.covariance.push_back(gps.eph); - fix2.covariance.push_back(gps.eph); - fix2.covariance.push_back(gps.eph); - // velocity variance -- Vxx, Vyy, Vzz - fix2.covariance.push_back(gps.s_variance_m_s); - fix2.covariance.push_back(gps.s_variance_m_s); - fix2.covariance.push_back(gps.s_variance_m_s); - - _gnss_fix2_publisher.broadcast(fix2); - } - } - - // optical_flow -> com::hex::equipment::flow::Measurement - if (_optical_flow_sub.updated()) { - optical_flow_s optical_flow; - - if (_optical_flow_sub.copy(&optical_flow)) { - com::hex::equipment::flow::Measurement measurement{}; - - measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s - measurement.rate_gyro_integral[0] = optical_flow.gyro_x_rate_integral; - measurement.rate_gyro_integral[1] = optical_flow.gyro_y_rate_integral; - measurement.flow_integral[0] = optical_flow.pixel_flow_x_integral; - measurement.flow_integral[1] = optical_flow.pixel_flow_y_integral; - measurement.quality = optical_flow.quality; - - _flow_measurement_publisher.broadcast(measurement); - } - } - - // safety -> standard::indication::button - if (_safety_sub.updated()) { - safety_s safety; - - if (_safety_sub.copy(&safety)) { - if (safety.safety_switch_available) { - standard::indication::Button Button{}; - Button.button = standard::indication::Button::BUTTON_SAFETY; - Button.press_time = safety.safety_off ? UINT8_MAX : 0; - _indication_button_publisher.broadcast(Button); - } - } - } + _node.spinOnce(); perf_end(_cycle_perf); @@ -546,7 +357,7 @@ void UavcanNode::Run() } } -void UavcanNode::print_info() +void UavcanNode::PrintInfo() { pthread_mutex_lock(&_node_mutex); @@ -581,6 +392,20 @@ void UavcanNode::print_info() printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); } + printf("\n"); + printf("Publishers:\n"); + + for (const auto &publisher : _publisher_list) { + publisher->PrintInfo(); + } + + printf("\n"); + printf("Subscribers:\n"); + + for (const auto &subscriber : _subscriber_list) { + subscriber->PrintInfo(); + } + printf("\n"); perf_print_counter(_cycle_perf); @@ -596,6 +421,8 @@ void UavcanNode::shrink() (void)pthread_mutex_unlock(&_node_mutex); } +} // namespace uavcannode + static void print_usage() { PX4_INFO("usage: \n" @@ -637,7 +464,7 @@ extern "C" int uavcannode_start(int argc, char *argv[]) // Start PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate); - int rv = UavcanNode::start(node_id, bitrate); + int rv = uavcannode::UavcanNode::start(node_id, bitrate); return rv; } @@ -650,7 +477,7 @@ extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "start")) { - if (UavcanNode::instance()) { + if (uavcannode::UavcanNode::instance()) { PX4_ERR("already started"); return 1; } @@ -659,7 +486,7 @@ extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]) } /* commands below require the app to be started */ - UavcanNode *const inst = UavcanNode::instance(); + uavcannode::UavcanNode *const inst = uavcannode::UavcanNode::instance(); if (!inst) { PX4_ERR("application not running"); @@ -668,7 +495,7 @@ extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { if (inst) { - inst->print_info(); + inst->PrintInfo(); } return 0; diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 3b4dddadb7..353bf87190 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -58,34 +58,24 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include #include #include -#include -#include +#include #include -#include -#include -#include -#include -#include -#include -#include + +#include "Publishers/UavcanPublisherBase.hpp" +#include "Subscribers/UavcanSubscriberBase.hpp" using namespace time_literals; +namespace uavcannode +{ + /** * A UAVCAN node. */ @@ -135,7 +125,7 @@ public: uavcan::Node<> &get_node() { return _node; } - void print_info(); + void PrintInfo(); void shrink(); @@ -147,11 +137,11 @@ public: /* The bit rate that can be passed back to the bootloader */ int32_t active_bitrate{0}; -protected: - void Run() override; private: - void fill_node_info(); - int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events); + void Run() override; + + void fill_node_info(); + int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events); px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver @@ -161,8 +151,8 @@ private: uavcan_node::Allocator _pool_allocator; - uavcan::Node<> _node; ///< library instance - pthread_mutex_t _node_mutex; + uavcan::Node<> _node; ///< library instance + pthread_mutex_t _node_mutex; uavcan::GlobalTimeSyncSlave _time_sync_slave; @@ -175,44 +165,19 @@ private: void cb_beginfirmware_update(const uavcan::ReceivedDataStructure &req, uavcan::ServiceResponseDataStructure &rsp); - - uavcan::Publisher _ahrs_magnetic_field_strength2_publisher; - uavcan::Publisher _gnss_fix2_publisher; - uavcan::Publisher _power_battery_info_publisher; - uavcan::Publisher _air_data_static_pressure_publisher; - uavcan::Publisher _air_data_static_temperature_publisher; - uavcan::Publisher _raw_air_data_publisher; - uavcan::Publisher _range_sensor_measurement; - uavcan::Publisher _flow_measurement_publisher; - uavcan::Publisher _indication_button_publisher; - - hrt_abstime _last_static_temperature_publish{0}; + IntrusiveSortedList _publisher_list; + IntrusiveSortedList _subscriber_list; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)}; - uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)}; - static constexpr int MAX_INSTANCES = 4; - uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[MAX_INSTANCES] { - {this, ORB_ID(distance_sensor), 0}, - {this, ORB_ID(distance_sensor), 1}, - {this, ORB_ID(distance_sensor), 2}, - {this, ORB_ID(distance_sensor), 3}, - }; - uORB::SubscriptionCallbackWorkItem _optical_flow_sub{this, ORB_ID(optical_flow)}; - uORB::SubscriptionCallbackWorkItem _safety_sub{this, ORB_ID(safety)}; - uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)}; - uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)}; - uORB::SubscriptionCallbackWorkItem _sensor_gps_sub{this, ORB_ID(sensor_gps)}; - UavcanNodeParamManager _param_manager; uavcan::ParamServer _param_server; - perf_counter_t _cycle_perf; - perf_counter_t _interval_perf; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; public: - /* A timer used to reboot after the response is sent */ uavcan::TimerEventForwarder _reset_timer; }; +}; // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNodeParamManager.cpp b/src/drivers/uavcannode/UavcanNodeParamManager.cpp index 4d80948ed7..f7e50c1f1a 100644 --- a/src/drivers/uavcannode/UavcanNodeParamManager.cpp +++ b/src/drivers/uavcannode/UavcanNodeParamManager.cpp @@ -36,6 +36,9 @@ #include #include +namespace uavcannode +{ + void UavcanNodeParamManager::getParamNameByIndex(Index index, Name &out_name) const { if (index < param_count_used()) { @@ -136,3 +139,5 @@ int UavcanNodeParamManager::eraseAllParams() param_reset_all(); return 0; } + +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNodeParamManager.hpp b/src/drivers/uavcannode/UavcanNodeParamManager.hpp index e29647ff46..dc3d3a5c0a 100644 --- a/src/drivers/uavcannode/UavcanNodeParamManager.hpp +++ b/src/drivers/uavcannode/UavcanNodeParamManager.hpp @@ -36,6 +36,9 @@ #include #include +namespace uavcannode +{ + class UavcanNodeParamManager : public uavcan::IParamManager { public: @@ -52,3 +55,4 @@ public: private: }; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/indication_controller.cpp b/src/drivers/uavcannode/indication_controller.cpp deleted file mode 100644 index d5741297e2..0000000000 --- a/src/drivers/uavcannode/indication_controller.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include "indication_controller.hpp" -#include -#include -#include -#include -#include -#include - -namespace -{ -unsigned self_light_index = 0; - -void cb_light_command(const uavcan::ReceivedDataStructure &msg) -{ - uavcan::uint32_t red = 0; - uavcan::uint32_t green = 0; - uavcan::uint32_t blue = 0; - - for (auto &cmd : msg.commands) { - if (cmd.light_id == self_light_index) { - using uavcan::equipment::indication::RGB565; - - red = uavcan::uint32_t(float(cmd.color.red) * - (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F); - - green = uavcan::uint32_t(float(cmd.color.green) * - (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F); - - blue = uavcan::uint32_t(float(cmd.color.blue) * - (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F); - - red = uavcan::min(red, 0xFFU); - green = uavcan::min(green, 0xFFU); - blue = uavcan::min(blue, 0xFFU); - - led_control_s led_control; - led_control.num_blinks = 0; - led_control.priority = led_control_s::MAX_PRIORITY; - led_control.mode = led_control_s::MODE_OFF; - led_control.led_mask = 0xff; - led_control.color = led_control_s::COLOR_OFF; - - if (red != 0 && blue == 0 && green == 0) { - led_control.color = led_control_s::COLOR_RED; - - } else if (red == 0 && blue != 0 && green == 0) { - led_control.color = led_control_s::COLOR_BLUE; - - } else if (red == 0 && blue == 0 && green != 0) { - led_control.color = led_control_s::COLOR_GREEN; - - } else if (red != 0 && blue == 0 && green != 0) { - led_control.color = led_control_s::COLOR_YELLOW; - - } else if (red != 0 && blue != 0 && green == 0) { - led_control.color = led_control_s::COLOR_PURPLE; - - } else if (red != 0 && blue == 0 && green != 0 && red > green) { - led_control.color = led_control_s::COLOR_AMBER; - - } else if (red == 0 && blue != 0 && green != 0) { - led_control.color = led_control_s::COLOR_CYAN; - - } else if (red != 0 && blue != 0 && green != 0) { - led_control.color = led_control_s::COLOR_WHITE; - } - - if (led_control.color != led_control_s::COLOR_OFF) { - led_control.mode = led_control_s::MODE_ON; - } - - led_control.timestamp = hrt_absolute_time(); - uORB::Publication led_control_pub{ORB_ID(led_control)}; - led_control_pub.publish(led_control); - } - } -} - -void cb_beep_command(const uavcan::ReceivedDataStructure &msg) -{ - tune_control_s tune_control{}; - tune_control.tune_id = 0; - tune_control.frequency = (uint16_t)msg.frequency; - tune_control.duration = uavcan::uint32_t(1000000 * msg.duration); - tune_control.volume = 0xff; - uORB::Publication tune_control_pub{ORB_ID(tune_control)}; - tune_control.timestamp = hrt_absolute_time(); - tune_control_pub.publish(tune_control); -} - - -} -int init_indication_controller(uavcan::INode &node) -{ - static uavcan::Subscriber sub_light(node); - static uavcan::Subscriber sub_beep(node); - - self_light_index = 0; - - int res = 0; - - res = sub_light.start(cb_light_command); - - if (res != 0) { - return res; - } - - res = sub_beep.start(cb_beep_command); - - if (res != 0) { - return res; - } - - return 0; -}