yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: added support for actuator_direct ORB topic

Browse Source 
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
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] = {};