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: implemented motor testing

Browse Source
This commit is contained in:
Holger Steinhaus
2014-11-03 18:47:07 +01:00
parent 58f36714f8
commit 0800fa4715
3 changed files with 29 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -279,6 +279,7 @@ int UavcanNode::run()
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,14 @@ int UavcanNode::run()
}
// can we mix?
if (controls_updated && (_mixers != nullptr)) {
if (_test_in_progress) {
float outputs[NUM_ACTUATOR_OUTPUTS] = {};
outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
// Output to the bus
_esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS);
}
else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
@@ -384,15 +392,27 @@ int UavcanNode::run()
}
}
// 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);
}