fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcannode: refactor into separate publishers and subscribers

Browse Source
This commit is contained in:
Daniel Agar 2021-01-27 20:57:23 -05:00 committed by GitHub
parent 39ad84e069
commit cf43d07f70
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 1286 additions and 450 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -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};

1
src/drivers/uavcannode/CMakeLists.txt
View File

@ -129,7 +129,6 @@ px4_add_module(
SRCS
allocator.hpp
uavcan_driver.hpp
indication_controller.cpp
UavcanNode.cpp
UavcanNode.hpp
UavcanNodeParamManager.hpp

102
src/drivers/uavcannode/Publishers/BatteryInfo.hpp Normal file
View File

@ -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

89
src/drivers/uavcannode/Publishers/FlowMeasurement.hpp Normal file
View File

@ -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

107
src/drivers/uavcannode/Publishers/GnssFix2.hpp Normal file
View File

@ -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

86
src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp Normal file
View File

@ -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

121
src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp Normal file
View File

@ -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

90
src/drivers/uavcannode/Publishers/RawAirData.hpp Normal file
View File

@ -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

83
src/drivers/uavcannode/Publishers/SafetyButton.hpp Normal file
View File

@ -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

83
src/drivers/uavcannode/Publishers/StaticPressure.hpp Normal file
View File

@ -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

87
src/drivers/uavcannode/Publishers/StaticTemperature.hpp Normal file
View File

@ -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

67
src/drivers/uavcannode/Publishers/UavcanPublisherBase.hpp Normal file
View File

@ -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

94
src/drivers/uavcannode/Subscribers/BeepCommand.hpp Normal file
View File

@ -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 <uavcan/equipment/indication/BeepCommand.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/tune_control.h>
namespace uavcannode
{
class BeepCommand;
typedef uavcan::MethodBinder<BeepCommand *,
void (BeepCommand::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::BeepCommand>&)>
BeepcommandBinder;
class BeepCommand :
public UavcanSubscriberBase,
private uavcan::Subscriber<uavcan::equipment::indication::BeepCommand, BeepcommandBinder>
{
public:
BeepCommand(uavcan::INode &node) :
UavcanSubscriberBase(uavcan::equipment::indication::BeepCommand::DefaultDataTypeID),
uavcan::Subscriber<uavcan::equipment::indication::BeepCommand, BeepcommandBinder>(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<uavcan::equipment::indication::BeepCommand> &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_s> _tune_control_pub{ORB_ID(tune_control)};
};
} // namespace uavcannode

149
src/drivers/uavcannode/Subscribers/LightsCommand.hpp Normal file
View File

@ -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 <uavcan/equipment/indication/LightsCommand.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/led_control.h>
namespace uavcannode
{
class LightsCommand;
typedef uavcan::MethodBinder<LightsCommand *,
void (LightsCommand::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand>&)>
LightscommandBinder;
class LightsCommand :
public UavcanSubscriberBase,
private uavcan::Subscriber<uavcan::equipment::indication::LightsCommand, LightscommandBinder>
{
public:
LightsCommand(uavcan::INode &node) :
UavcanSubscriberBase(uavcan::equipment::indication::LightsCommand::DefaultDataTypeID),
uavcan::Subscriber<uavcan::equipment::indication::LightsCommand, LightscommandBinder>(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<uavcan::equipment::indication::LightsCommand> &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<uavcan::uint32_t>(red, 0xFFU);
green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
blue = uavcan::min<uavcan::uint32_t>(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_s> _led_control_pub{ORB_ID(led_control)};
unsigned _self_light_index{0};
};
} // namespace uavcannode

35
src/drivers/uavcannode/indication_controller.hpp → src/drivers/uavcannode/Subscribers/UavcanSubscriberBase.hpp
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
* 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 <containers/IntrusiveSortedList.hpp>
#include <uavcan/uavcan.hpp>
int init_indication_controller(uavcan::INode &node);
#include <uavcan/node/subscriber.hpp>
namespace uavcannode
{
class UavcanSubscriberBase : public IntrusiveSortedListNode<UavcanSubscriberBase *>
{
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

305
src/drivers/uavcannode/UavcanNode.cpp
View File

@ -32,15 +32,30 @@
****************************************************************************/
#include "UavcanNode.hpp"
#include "indication_controller.hpp"
#include "boot_app_shared.h"
#include <lib/ecl/geo/geo.h>
#include <lib/version/version.h>
#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;

75
src/drivers/uavcannode/UavcanNode.hpp
View File

@ -58,34 +58,24 @@
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/protocol/RestartNode.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
#include <uavcan/equipment/air_data/RawAirData.hpp>
#include <uavcan/equipment/air_data/StaticPressure.hpp>
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <uavcan/equipment/range_sensor/Measurement.hpp>
#include <standard/indication/Button.hpp>
#include <com/hex/equipment/flow/Measurement.hpp>
#include <containers/IntrusiveSortedList.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_gps.h>
#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<UavcanNode::BeginFirmwareUpdate::Request> &req,
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp);
uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2> _ahrs_magnetic_field_strength2_publisher;
uavcan::Publisher<uavcan::equipment::gnss::Fix2> _gnss_fix2_publisher;
uavcan::Publisher<uavcan::equipment::power::BatteryInfo> _power_battery_info_publisher;
uavcan::Publisher<uavcan::equipment::air_data::StaticPressure> _air_data_static_pressure_publisher;
uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature> _air_data_static_temperature_publisher;
uavcan::Publisher<uavcan::equipment::air_data::RawAirData> _raw_air_data_publisher;
uavcan::Publisher<uavcan::equipment::range_sensor::Measurement> _range_sensor_measurement;
uavcan::Publisher<com::hex::equipment::flow::Measurement> _flow_measurement_publisher;
uavcan::Publisher<standard::indication::Button> _indication_button_publisher;
hrt_abstime _last_static_temperature_publish{0};
IntrusiveSortedList<UavcanPublisherBase *> _publisher_list;
IntrusiveSortedList<UavcanSubscriberBase *> _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<void (*)(const uavcan::TimerEvent &)> _reset_timer;
};
}; // namespace uavcannode

5
src/drivers/uavcannode/UavcanNodeParamManager.cpp
View File

@ -36,6 +36,9 @@
#include <px4_platform_common/defines.h>
#include <lib/parameters/param.h>
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

4
src/drivers/uavcannode/UavcanNodeParamManager.hpp
View File

@ -36,6 +36,9 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/protocol/param_server.hpp>
namespace uavcannode
{
class UavcanNodeParamManager : public uavcan::IParamManager
{
public:
@ -52,3 +55,4 @@ public:
private:
};
} // namespace uavcannode

151
src/drivers/uavcannode/indication_controller.cpp
View File

@ -1,151 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
*
* 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 <px4_platform_common/px4_config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include "indication_controller.hpp"
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>
#include <lib/tunes/tunes.h>
namespace
{
unsigned self_light_index = 0;
void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand> &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<uavcan::uint32_t>(red, 0xFFU);
green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
blue = uavcan::min<uavcan::uint32_t>(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_s> led_control_pub{ORB_ID(led_control)};
led_control_pub.publish(led_control);
}
}
}
void cb_beep_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::BeepCommand> &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_s> 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<uavcan::equipment::indication::LightsCommand> sub_light(node);
static uavcan::Subscriber<uavcan::equipment::indication::BeepCommand> 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;
}