aboutsummaryrefslogtreecommitdiff
path: root/src/modules/uavcan/uavcan_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/uavcan/uavcan_main.cpp')
-rw-r--r--src/modules/uavcan/uavcan_main.cpp135
1 files changed, 77 insertions, 58 deletions
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 4535b6d6a..a8485ee44 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -38,8 +38,10 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
+#include <systemlib/scheduling_priorities.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
@@ -65,16 +67,18 @@ UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
- _esc_controller(_node),
- _gnss_receiver(_node)
+ _node_mutex(),
+ _esc_controller(_node)
{
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
- // memset(_controls, 0, sizeof(_controls));
- // memset(_poll_fds, 0, sizeof(_poll_fds));
+ const int res = pthread_mutex_init(&_node_mutex, nullptr);
+ if (res < 0) {
+ std::abort();
+ }
}
UavcanNode::~UavcanNode()
@@ -99,10 +103,18 @@ UavcanNode::~UavcanNode()
}
/* clean up the alternate device node */
- // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+ // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
+ // Removing the sensor bridges
+ auto br = _sensor_bridges.getHead();
+ while (br != nullptr) {
+ auto next = br->getSibling();
+ delete br;
+ br = next;
+ }
+
_instance = nullptr;
}
@@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
- _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+ _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
@@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
- /* do regular cdev init */
+ // Do regular cdev init
ret = CDev::init();
- if (ret != OK)
+ if (ret != OK) {
return ret;
+ }
_node.setName("org.pixhawk.pixhawk");
@@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
fill_node_info();
- /* initializing the bridges UAVCAN <--> uORB */
+ // Actuators
ret = _esc_controller.init();
- if (ret < 0)
+ if (ret < 0) {
return ret;
+ }
- ret = _gnss_receiver.init();
- if (ret < 0)
- return ret;
+ // Sensor bridges
+ IUavcanSensorBridge::make_all(_node, _sensor_bridges);
+ auto br = _sensor_bridges.getHead();
+ while (br != nullptr) {
+ ret = br->init();
+ if (ret < 0) {
+ warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
+ return ret;
+ }
+ warnx("sensor bridge '%s' init ok", br->get_name());
+ br = br->getSibling();
+ }
return _node.start();
}
@@ -248,6 +271,8 @@ void UavcanNode::node_spin_once()
int UavcanNode::run()
{
+ (void)pthread_mutex_lock(&_node_mutex);
+
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
@@ -291,8 +316,13 @@ int UavcanNode::run()
_groups_subscribed = _groups_required;
}
+ // Mutex is unlocked while the thread is blocked on IO multiplexing
+ (void)pthread_mutex_unlock(&_node_mutex);
+
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
+ (void)pthread_mutex_lock(&_node_mutex);
+
node_spin_once(); // Non-blocking
// this would be bad...
@@ -352,7 +382,6 @@ int UavcanNode::run()
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
-
}
// Check arming state
@@ -376,10 +405,7 @@ int UavcanNode::run()
}
int
-UavcanNode::control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input)
+UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@@ -520,8 +546,23 @@ UavcanNode::print_info()
warnx("not running, start first");
}
- warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
- warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+ (void)pthread_mutex_lock(&_node_mutex);
+
+ // ESC mixer status
+ printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+ (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+ printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
+
+ // Sensor bridges
+ auto br = _sensor_bridges.getHead();
+ while (br != nullptr) {
+ printf("Sensor '%s':\n", br->get_name());
+ br->print_status();
+ printf("\n");
+ br = br->getSibling();
+ }
+
+ (void)pthread_mutex_unlock(&_node_mutex);
}
/*
@@ -529,79 +570,57 @@ UavcanNode::print_info()
*/
static void print_usage()
{
- warnx("usage: uavcan start <node_id> [can_bitrate]");
+ warnx("usage: \n"
+ "\tuavcan {start|status|stop}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
{
- constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
-
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
- if (argc < 3) {
- print_usage();
- ::exit(1);
+ if (UavcanNode::instance()) {
+ errx(1, "already started");
}
- /*
- * Node ID
- */
- const int node_id = atoi(argv[2]);
+ // Node ID
+ int32_t node_id = 0;
+ (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
- /*
- * CAN bitrate
- */
- unsigned bitrate = 0;
-
- if (argc > 3) {
- bitrate = atol(argv[3]);
- }
-
- if (bitrate <= 0) {
- bitrate = DEFAULT_CAN_BITRATE;
- }
+ // CAN bitrate
+ int32_t bitrate = 0;
+ (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
- if (UavcanNode::instance()) {
- errx(1, "already started");
- }
-
- /*
- * Start
- */
+ // Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
-
}
/* commands below require the app to be started */
- UavcanNode *inst = UavcanNode::instance();
+ UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
-
- inst->print_info();
- return OK;
+ inst->print_info();
+ ::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
-
- delete inst;
- inst = nullptr;
- return OK;
+ delete inst;
+ ::exit(0);
}
print_usage();
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-28 03:45:19 +0000