mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 09:33:33 +08:00
uavcannode: refactor into separate publishers and subscribers
This commit is contained in:
Daniel Agar
@@ -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
|
||||
@@ -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
|
||||
@@ -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/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
|
||||
Reference in New Issue
Block a user