dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uavcan: implement servo outputs

This commit is contained in:
Beat Küng 2021-09-17 11:19:34 +02:00 committed by Daniel Agar
parent 07fa8c5295
commit 3ff6014a3c
6 changed files with 215 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -144,6 +144,7 @@ px4_add_module(
# Actuators
actuators/esc.cpp
actuators/hardpoint.cpp
actuators/servo.cpp
# Sensors
sensors/sensor_bridge.cpp

62
src/drivers/uavcan/actuators/servo.cpp Normal file
View File

@ -0,0 +1,62 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#include "servo.hpp"
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
UavcanServoController::UavcanServoController(uavcan::INode &node) :
_node(node),
_uavcan_pub_array_cmd(node)
{
_uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
}
void
UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
{
uavcan::equipment::actuator::ArrayCommand msg;
for (unsigned i = 0; i < num_outputs; ++i) {
uavcan::equipment::actuator::Command cmd;
cmd.actuator_id = i;
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
msg.commands.push_back(cmd);
}
_uavcan_pub_array_cmd.broadcast(msg);
}

60
src/drivers/uavcan/actuators/servo.hpp Normal file
View File

@ -0,0 +1,60 @@
/****************************************************************************
*
* 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 <uavcan/uavcan.hpp>
#include <uavcan/equipment/actuator/Status.hpp>
#include <uavcan/equipment/actuator/ArrayCommand.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
class UavcanServoController
{
public:
static constexpr int MAX_ACTUATORS = 8;
static constexpr unsigned MAX_RATE_HZ = 50;
static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 6; ///< 0..31, inclusive, 0 - highest, 31 - lowest
UavcanServoController(uavcan::INode &node);
~UavcanServoController() = default;
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
private:
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand> _uavcan_pub_array_cmd;
};

8
src/drivers/uavcan/module.yaml
View File

@ -9,4 +9,12 @@ actuator_output:
max: { min: 0, max: 8191, default: 8191 }
failsafe: { min: 0, max: 8191 }
num_channels: 8
- param_prefix: UAVCAN_SV
channel_label: 'UAVCAN Servo'
standard_params:
disarmed: { min: 0, max: 1000, default: 500 }
min: { min: 0, max: 1000, default: 0 }
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 8

65
src/drivers/uavcan/uavcan_main.cpp
View File

@ -82,6 +82,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
ModuleParams(nullptr),
_node(can_driver, system_clock, _pool_allocator),
_esc_controller(_node),
_servo_controller(_node),
_hardpoint_controller(_node),
_beep_controller(_node),
_safety_state_controller(_node),
@ -107,6 +108,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
/* _server_command_sem use case is a signal */
px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
}
UavcanNode::~UavcanNode()
@ -413,7 +418,8 @@ UavcanNode::get_param(int remote_node_id, const char *name)
void
UavcanNode::update_params()
{
_mixing_interface.updateParams();
_mixing_interface_esc.updateParams();
_mixing_interface_servo.updateParams();
}
int
@ -561,7 +567,8 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
}
_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
_instance->_mixing_interface.ScheduleNow();
_instance->_mixing_interface_esc.ScheduleNow();
_instance->_mixing_interface_servo.ScheduleNow();
return OK;
}
@ -669,20 +676,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
return -EINVAL;
}
_mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
// these are configurable with dynamic mixing
_mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
_mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
_mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
_mixing_interface_esc.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
enable_idle_throttle_when_armed(true);
}
_mixing_interface.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
/* Start the Node */
return _node.start();
}
@ -788,7 +792,7 @@ UavcanNode::Run()
node_spin_once(); // expected to be non-blocking
// Check arming state
const actuator_armed_s &armed = _mixing_interface.mixingOutput().armed();
const actuator_armed_s &armed = _mixing_interface_esc.mixingOutput().armed();
enable_idle_throttle_when_armed(!armed.soft_stop);
// check for parameter updates
@ -824,8 +828,10 @@ UavcanNode::Run()
pthread_mutex_unlock(&_node_mutex);
if (_task_should_exit.load()) {
_mixing_interface.mixingOutput().unregister();
_mixing_interface.ScheduleClear();
_mixing_interface_esc.mixingOutput().unregister();
_mixing_interface_esc.ScheduleClear();
_mixing_interface_servo.mixingOutput().unregister();
_mixing_interface_servo.ScheduleClear();
ScheduleClear();
teardown();
_instance = nullptr;
@ -837,9 +843,9 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
{
value &= _idle_throttle_when_armed_param > 0;
if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
if (value != _idle_throttle_when_armed) {
_mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0);
_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
_idle_throttle_when_armed = value;
}
}
@ -867,14 +873,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
break;
case MIXERIOCRESET:
_mixing_interface.mixingOutput().resetMixerThreadSafe();
_mixing_interface_esc.mixingOutput().resetMixerThreadSafe();
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strlen(buf);
ret = _mixing_interface.mixingOutput().loadMixerThreadSafe(buf, buflen);
ret = _mixing_interface_esc.mixingOutput().loadMixerThreadSafe(buf, buflen);
}
break;
@ -920,14 +926,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
}
bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
return true;
}
void UavcanMixingInterface::Run()
void UavcanMixingInterfaceESC::Run()
{
pthread_mutex_lock(&_node_mutex);
_mixing_output.update();
@ -935,7 +941,7 @@ void UavcanMixingInterface::Run()
pthread_mutex_unlock(&_node_mutex);
}
void UavcanMixingInterface::mixerChanged()
void UavcanMixingInterfaceESC::mixerChanged()
{
int rotor_count = 0;
@ -953,6 +959,21 @@ void UavcanMixingInterface::mixerChanged()
_esc_controller.set_rotor_count(rotor_count);
}
bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
_servo_controller.update_outputs(stop_motors, outputs, num_outputs);
return true;
}
void UavcanMixingInterfaceServo::Run()
{
pthread_mutex_lock(&_node_mutex);
_mixing_output.update();
_mixing_output.updateSubscriptions(false);
pthread_mutex_unlock(&_node_mutex);
}
void
UavcanNode::print_info()
{
@ -994,8 +1015,10 @@ UavcanNode::print_info()
printf("\n");
// ESC mixer status
_mixing_interface.mixingOutput().printStatus();
printf("ESC outputs:\n");
_mixing_interface_esc.mixingOutput().printStatus();
printf("Servo outputs:\n");
_mixing_interface_servo.mixingOutput().printStatus();
printf("\n");

48
src/drivers/uavcan/uavcan_main.hpp
View File

@ -54,6 +54,7 @@
#include "allocator.hpp"
#include "actuators/esc.hpp"
#include "actuators/hardpoint.hpp"
#include "actuators/servo.hpp"
#include "sensors/sensor_bridge.hpp"
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
@ -77,17 +78,17 @@ using namespace time_literals;
class UavcanNode;
/**
* UAVCAN mixing class.
* It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
* UAVCAN mixing class for ESCs.
* It is separate from UavcanNode to have separate WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterfaceESC runs upon actuator_control updates, whereas UavcanNode runs at
* a fixed rate or upon bus updates).
* Both work items are expected to run on the same work queue.
* All work items are expected to run on the same work queue.
*/
class UavcanMixingInterface : public OutputModuleInterface
class UavcanMixingInterfaceESC : public OutputModuleInterface
{
public:
UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
UavcanMixingInterfaceESC(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
: OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::uavcan),
_node_mutex(node_mutex),
_esc_controller(esc_controller) {}
@ -107,6 +108,35 @@ private:
MixingOutput _mixing_output{"UAVCAN_EC", UavcanEscController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
};
/**
* UAVCAN mixing class for Servos.
* It is separate from UavcanNode to have separate WorkItems and therefore allowing independent scheduling
* (I.e. UavcanMixingInterfaceServo runs upon actuator_control updates, whereas UavcanNode runs at
* a fixed rate or upon bus updates).
* All work items are expected to run on the same work queue.
*/
class UavcanMixingInterfaceServo : public OutputModuleInterface
{
public:
UavcanMixingInterfaceServo(pthread_mutex_t &node_mutex, UavcanServoController &servo_controller)
: OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::uavcan),
_node_mutex(node_mutex),
_servo_controller(servo_controller) {}
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
MixingOutput &mixingOutput() { return _mixing_output; }
protected:
void Run() override;
private:
friend class UavcanNode;
pthread_mutex_t &_node_mutex;
UavcanServoController &_servo_controller;
MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
};
/**
* A UAVCAN node.
*/
@ -204,7 +234,9 @@ private:
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller};
UavcanServoController _servo_controller;
UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller};
UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller};
UavcanHardpointController _hardpoint_controller;
UavcanBeep _beep_controller;
UavcanSafetyState _safety_state_controller;