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.cpp170
1 files changed, 125 insertions, 45 deletions
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 2c543462e..60901e0c7 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -46,6 +46,8 @@
#include <arch/board/board.h>
#include <arch/chip/chip.h>
+#include <uORB/topics/esc_status.h>
+
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
@@ -269,6 +271,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);
@@ -280,9 +300,9 @@ int UavcanNode::run()
_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)
@@ -304,11 +324,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
@@ -326,6 +350,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);
@@ -333,24 +359,39 @@ 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 (_test_in_progress) {
- float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
- test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
-
- // Output to the bus
- _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+ 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,
@@ -358,39 +399,41 @@ int UavcanNode::run()
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 motor test state
bool updated = false;
orb_check(_test_motor_sub, &updated);
@@ -459,7 +502,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);
@@ -472,9 +514,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]);
}
}
}
@@ -572,6 +612,36 @@ 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");
+
+ // ESC status
+ int esc_sub = orb_subscribe(ORB_ID(esc_status));
+ struct esc_status_s esc;
+ memset(&esc, 0, sizeof(esc));
+ orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+ printf("ESC Status:\n");
+ printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+ for (uint8_t i=0; i<_outputs.noutputs; i++) {
+ printf("%d\t", esc.esc[i].esc_address);
+ printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+ printf("%3.2f\t", (double)esc.esc[i].esc_current);
+ printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+ printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+ printf("%d\t", esc.esc[i].esc_rpm);
+ printf("%d", esc.esc[i].esc_errorcount);
+ printf("\n");
+ }
+
+ orb_unsubscribe(esc_sub);
+ }
+
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
@@ -590,7 +660,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[]);
@@ -637,6 +707,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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-02 23:50:26 +0000