aboutsummaryrefslogtreecommitdiff
path: root/src/modules/uavcan
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/uavcan')
-rw-r--r--src/modules/uavcan/actuators/esc.cpp44
-rw-r--r--src/modules/uavcan/actuators/esc.hpp8
-rw-r--r--src/modules/uavcan/sensors/baro.cpp6
-rw-r--r--src/modules/uavcan/sensors/gnss.cpp2
-rw-r--r--src/modules/uavcan/sensors/gnss.hpp2
-rw-r--r--src/modules/uavcan/sensors/mag.cpp53
-rw-r--r--src/modules/uavcan/sensors/mag.hpp2
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.cpp3
-rw-r--r--src/modules/uavcan/sensors/sensor_bridge.hpp2
-rw-r--r--src/modules/uavcan/uavcan_main.cpp165
-rw-r--r--src/modules/uavcan/uavcan_main.hpp22
11 files changed, 231 insertions, 78 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef..9f682c7e1 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
#include "esc.hpp"
#include <systemlib/err.h>
+
+#define MOTOR_BIT(x) (1<<(x))
+
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
@@ -73,7 +76,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
- if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
+ if ((outputs == nullptr) ||
+ (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+ (num_outputs > CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -95,14 +100,18 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
- if (_armed) {
- for (unsigned i = 0; i < num_outputs; i++) {
-
+ for (unsigned i = 0; i < num_outputs; i++) {
+ if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
- if (scaled < 1.0F) {
- scaled = 1.0F; // Since we're armed, we don't want to stop it completely
- }
-
+ // trim negative values back to 0. Previously
+ // we set this to 0.1, which meant motors kept
+ // spinning when armed, but that should be a
+ // policy decision for a specific vehicle
+ // type, as it is not appropriate for all
+ // types of vehicles (eg. fixed wing).
+ if (scaled < 0.0F) {
+ scaled = 0.0F;
+ }
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
@@ -111,6 +120,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+ } else {
+ msg.cmd.push_back(static_cast<unsigned>(0));
}
}
@@ -121,9 +132,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
+{
+ if (arm) {
+ _armed_mask = -1;
+ } else {
+ _armed_mask = 0;
+ }
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
{
- _armed = arm;
+ if (arm) {
+ _armed_mask = MOTOR_BIT(num);
+ } else {
+ _armed_mask = 0;
+ }
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e6..12c035542 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
void update_outputs(float *outputs, unsigned num_outputs);
- void arm_esc(bool arm);
+ void arm_all_escs(bool arm);
+ void arm_single_esc(int num, bool arm);
private:
/**
@@ -99,6 +100,11 @@ private:
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
+ * ESC states
+ */
+ uint32_t _armed_mask = 0;
+
+ /*
* Perf counters
*/
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828..8741ae20d 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
{
auto report = ::baro_report();
- report.timestamp = msg.getUtcTimestamp().toUSec();
- if (report.timestamp == 0) {
- report.timestamp = msg.getMonotonicTimestamp().toUSec();
- }
-
+ report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf..a375db37f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
auto report = ::vehicle_gps_position_s();
- report.timestamp_position = hrt_absolute_time();
+ report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401..2111ff80b 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
#pragma once
-#include <drivers/drv_hrt.h>
-
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f..35ebee542 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,8 @@
#include "mag.hpp"
+#include <systemlib/err.h>
+
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
@@ -49,6 +51,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
+ _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
@@ -69,9 +73,36 @@ int UavcanMagnetometerBridge::init()
return 0;
}
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+ static uint64_t last_read = 0;
+ struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+ /* buffer must be large enough */
+ unsigned count = buflen / sizeof(struct mag_report);
+ if (count < 1) {
+ return -ENOSPC;
+ }
+
+ if (last_read < _report.timestamp) {
+ /* copy report */
+ lock();
+ *mag_buf = _report;
+ last_read = _report.timestamp;
+ unlock();
+ return sizeof(struct mag_report);
+ } else {
+ /* no new data available, warn caller */
+ return -EAGAIN;
+ }
+}
+
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
+ case SENSORIOCSQUEUEDEPTH: {
+ return OK; // Pretend that this stuff is supported to keep APM happy
+ }
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
@@ -84,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
- return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard
+ return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
@@ -106,18 +137,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
{
- auto report = ::mag_report();
-
- report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
-
- report.timestamp = msg.getUtcTimestamp().toUSec();
- if (report.timestamp == 0) {
- report.timestamp = msg.getMonotonicTimestamp().toUSec();
- }
+ lock();
+ _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
+ _report.timestamp = msg.getMonotonicTimestamp().toUSec();
- report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
- report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
- report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+ _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+ _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+ _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+ unlock();
- publish(msg.getSrcNodeID().get(), &report);
+ publish(msg.getSrcNodeID().get(), &_report);
}
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f7..74077d883 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -54,6 +54,7 @@ public:
int init() override;
private:
+ ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
mag_scale _scale = {};
+ mag_report _report = {};
};
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 9608ce680..0999938fc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
return;
}
+ // update device id as we now know our device node_id
+ _device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 1316f7ecc..e31960537 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -112,6 +112,8 @@ protected:
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+ _device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+ _device_id.devid_s.bus = 0;
}
/**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a8485ee44..8147a8b89 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -269,6 +269,24 @@ void UavcanNode::node_spin_once()
}
}
+/*
+ add a fd to the list of polled events. This assumes you want
+ POLLIN for now.
+ */
+int UavcanNode::add_poll_fd(int fd)
+{
+ int ret = _poll_fds_num;
+ if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
+ errx(1, "uavcan: too many poll fds, exiting");
+ }
+ _poll_fds[_poll_fds_num] = ::pollfd();
+ _poll_fds[_poll_fds_num].fd = fd;
+ _poll_fds[_poll_fds_num].events = POLLIN;
+ _poll_fds_num += 1;
+ return ret;
+}
+
+
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
@@ -279,9 +297,10 @@ int UavcanNode::run()
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ _test_motor_sub = orb_subscribe(ORB_ID(test_motor));
+ _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
- actuator_outputs_s outputs;
- memset(&outputs, 0, sizeof(outputs));
+ memset(&_outputs, 0, sizeof(_outputs));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0)
@@ -303,11 +322,15 @@ int UavcanNode::run()
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
- _poll_fds_num = 0;
- _poll_fds[_poll_fds_num] = ::pollfd();
- _poll_fds[_poll_fds_num].fd = busevent_fd;
- _poll_fds[_poll_fds_num].events = POLLIN;
- _poll_fds_num += 1;
+ add_poll_fd(busevent_fd);
+
+ /*
+ * setup poll to look for actuator direct input if we are
+ * subscribed to the topic
+ */
+ if (_actuator_direct_sub != -1) {
+ _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
+ }
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
@@ -325,6 +348,8 @@ int UavcanNode::run()
node_spin_once(); // Non-blocking
+ bool new_output = false;
+
// this would be bad...
if (poll_ret < 0) {
log("poll error %d", errno);
@@ -332,67 +357,102 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
- unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
- if (_poll_fds[poll_id].revents & POLLIN) {
+ if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
controls_updated = true;
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
- poll_id++;
}
}
+ /*
+ see if we have any direct actuator updates
+ */
+ if (_actuator_direct_sub != -1 &&
+ (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
+ orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
+ !_test_in_progress) {
+ if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
+ _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
+ }
+ memcpy(&_outputs.output[0], &_actuator_direct.values[0],
+ _actuator_direct.nvalues*sizeof(float));
+ _outputs.noutputs = _actuator_direct.nvalues;
+ new_output = true;
+ }
+
// can we mix?
- if (controls_updated && (_mixers != nullptr)) {
+ if (_test_in_progress) {
+ memset(&_outputs, 0, sizeof(_outputs));
+ if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) {
+ _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+ _outputs.noutputs = _test_motor.motor_number+1;
+ }
+ new_output = true;
+ } else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
unsigned num_outputs_max = 8;
// Do mixing
- outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
- outputs.timestamp = hrt_absolute_time();
-
- // iterate actuators
- for (unsigned i = 0; i < outputs.noutputs; i++) {
- // last resort: catch NaN, INF and out-of-band errors
- if (!isfinite(outputs.output[i])) {
- /*
- * Value is NaN, INF or out of band - set to the minimum value.
- * This will be clearly visible on the servo status and will limit the risk of accidentally
- * spinning motors. It would be deadly in flight.
- */
- outputs.output[i] = -1.0f;
- }
+ _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
- // limit outputs to valid range
+ new_output = true;
+ }
+ }
- // never go below min
- if (outputs.output[i] < -1.0f) {
- outputs.output[i] = -1.0f;
- }
+ if (new_output) {
+ // iterate actuators, checking for valid values
+ for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+ // last resort: catch NaN, INF and out-of-band errors
+ if (!isfinite(_outputs.output[i])) {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ _outputs.output[i] = -1.0f;
+ }
- // never go below max
- if (outputs.output[i] > 1.0f) {
- outputs.output[i] = 1.0f;
- }
+ // never go below min
+ if (_outputs.output[i] < -1.0f) {
+ _outputs.output[i] = -1.0f;
}
- // Output to the bus
- _esc_controller.update_outputs(outputs.output, outputs.noutputs);
+ // never go above max
+ if (_outputs.output[i] > 1.0f) {
+ _outputs.output[i] = 1.0f;
+ }
}
+ // Output to the bus
+ _outputs.timestamp = hrt_absolute_time();
+ _esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
}
- // Check arming state
+
+ // Check motor test state
bool updated = false;
+ orb_check(_test_motor_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+ // Update the test status and check that we're not locked down
+ _test_in_progress = (_test_motor.value > 0);
+ _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+ }
+
+ // Check arming state
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
- // Update the armed status and check that we're not locked down
- bool set_armed = _armed.armed && !_armed.lockdown;
+ // Update the armed status and check that we're not locked down and motor
+ // test is not running
+ bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
arm_actuators(set_armed);
}
@@ -429,7 +489,7 @@ int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
- _esc_controller.arm_esc(arm);
+ _esc_controller.arm_all_escs(arm);
return OK;
}
@@ -440,7 +500,6 @@ UavcanNode::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
// the first fd used by CAN
- _poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@@ -453,9 +512,7 @@ UavcanNode::subscribe()
}
if (_control_subs[i] > 0) {
- _poll_fds[_poll_fds_num].fd = _control_subs[i];
- _poll_fds[_poll_fds_num].events = POLLIN;
- _poll_fds_num++;
+ _poll_ids[i] = add_poll_fd(_control_subs[i]);
}
}
}
@@ -553,6 +610,14 @@ UavcanNode::print_info()
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
+ if (_outputs.noutputs != 0) {
+ printf("ESC output: ");
+ for (uint8_t i=0; i<_outputs.noutputs; i++) {
+ printf("%d ", (int)(_outputs.output[i]*1000));
+ }
+ printf("\n");
+ }
+
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
@@ -571,7 +636,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
- "\tuavcan {start|status|stop}");
+ "\tuavcan {start|status|stop|arm|disarm}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -618,6 +683,16 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
+ if (!std::strcmp(argv[1], "arm")) {
+ inst->arm_actuators(true);
+ ::exit(0);
+ }
+
+ if (!std::strcmp(argv[1], "disarm")) {
+ inst->arm_actuators(false);
+ ::exit(0);
+ }
+
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db9741..98f2e5ad4 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,8 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
@@ -56,6 +58,9 @@
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
+// we add two to allow for actuator_direct and busevent
+#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
+
/**
* A UAVCAN node.
*/
@@ -96,6 +101,8 @@ private:
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
+ int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
+
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
@@ -103,6 +110,10 @@ private:
actuator_armed_s _armed; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
+ int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
+ test_motor_s _test_motor;
+ bool _test_in_progress = false;
+
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
@@ -120,6 +131,15 @@ private:
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
- pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
+ pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
unsigned _poll_fds_num = 0;
+
+ int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic
+ uint8_t _actuator_direct_poll_fd_num;
+ actuator_direct_s _actuator_direct;
+
+ actuator_outputs_s _outputs;
+
+ // index into _poll_fds for each _control_subs handle
+ uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 16:44:10 +0000