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

uavcan: added support for actuator_direct ORB topic 

this watches the actuator_direct topic and uses it to allow for direct
output of actuator values, bypassing the mixer
This commit is contained in:
Andrew Tridgell 2014-11-20 09:37:02 +11:00
parent f6b0a3e07f
commit b830137ec8
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/uavcan/uavcan_main.cpp
View File

@ -280,6 +280,7 @@ 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));
@ -310,6 +311,18 @@ int UavcanNode::run()
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
/*
* setup poll to look for actuator direct input if we are
* subscribed to the topic
*/
if (_actuator_direct_sub != -1) {
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = _actuator_direct_sub;
_poll_fds[_poll_fds_num].events = POLLIN;
_actuator_direct_poll_fd_num = _poll_fds_num;
_poll_fds_num += 1;
}
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
@ -342,6 +355,16 @@ int UavcanNode::run()
}
}
/*
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) {
// Output to the bus
_esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues);
}
// can we mix?
if (_test_in_progress) {
float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};

5
src/modules/uavcan/uavcan_main.hpp
View File

@ -42,6 +42,7 @@
#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"
@ -128,6 +129,10 @@ private:
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
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;
// index into _poll_fds for each _control_subs handle
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
};