mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-28 02:13:34 +08:00
uavcannode: refactor into separate publishers and subscribers
This commit is contained in:
Daniel Agar
@@ -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 <uavcan/equipment/power/BatteryInfo.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class BatteryInfo :
|
||||
public UavcanPublisherBase,
|
||||
private uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::power::BatteryInfo>
|
||||
{
|
||||
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<uavcan::equipment::power::BatteryInfo>(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<uavcan::equipment::power::BatteryInfo>::broadcast(battery_info);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcan
|
||||
@@ -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 <com/hex/equipment/flow/Measurement.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class FlowMeasurement :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<com::hex::equipment::flow::Measurement>
|
||||
{
|
||||
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<com::hex::equipment::flow::Measurement>(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<com::hex::equipment::flow::Measurement>::broadcast(measurement);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/gnss/Fix2.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class GnssFix2 :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::gnss::Fix2>
|
||||
{
|
||||
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<uavcan::equipment::gnss::Fix2>(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<uavcan::equipment::gnss::Fix2>::broadcast(fix2);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/sensor_mag.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class MagneticFieldStrength2 :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2>
|
||||
{
|
||||
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<uavcan::equipment::ahrs::MagneticFieldStrength2>(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<uavcan::equipment::ahrs::MagneticFieldStrength2>::broadcast(magnetic_field);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/range_sensor/Measurement.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class RangeSensorMeasurement :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::range_sensor::Measurement>
|
||||
{
|
||||
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<uavcan::equipment::range_sensor::Measurement>(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<uavcan::equipment::range_sensor::Measurement>::broadcast(range_sensor);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/air_data/RawAirData.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class RawAirData :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::air_data::RawAirData>
|
||||
{
|
||||
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<uavcan::equipment::air_data::RawAirData>(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<uavcan::equipment::air_data::RawAirData>::broadcast(raw_air_data);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <standard/indication/Button.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/safety.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class SafetyButton :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<standard::indication::Button>
|
||||
{
|
||||
public:
|
||||
SafetyButton(px4::WorkItem *work_item, uavcan::INode &node) :
|
||||
UavcanPublisherBase(standard::indication::Button::DefaultDataTypeID),
|
||||
uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)),
|
||||
uavcan::Publisher<standard::indication::Button>(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<standard::indication::Button>::broadcast(Button);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/air_data/StaticPressure.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class StaticPressure :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::air_data::StaticPressure>
|
||||
{
|
||||
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<uavcan::equipment::air_data::StaticPressure>(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<uavcan::equipment::air_data::StaticPressure>::broadcast(static_pressure);
|
||||
|
||||
// ensure callback is registered
|
||||
uORB::SubscriptionCallbackWorkItem::registerCallback();
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace uavcannode
|
||||
@@ -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 <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class StaticTemperature :
|
||||
public UavcanPublisherBase,
|
||||
public uORB::SubscriptionCallbackWorkItem,
|
||||
private uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature>
|
||||
{
|
||||
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<uavcan::equipment::air_data::StaticTemperature>(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<uavcan::equipment::air_data::StaticTemperature>::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
|
||||
@@ -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 <containers/IntrusiveSortedList.hpp>
|
||||
#include <uavcan/uavcan.hpp>
|
||||
|
||||
#include <uavcan/node/publisher.hpp>
|
||||
|
||||
namespace uavcannode
|
||||
{
|
||||
|
||||
class UavcanPublisherBase : public IntrusiveSortedListNode<UavcanPublisherBase *>
|
||||
{
|
||||
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
|
||||
Reference in New Issue
Block a user