diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-22 12:47:08 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-22 12:47:08 +0100 |
commit | f3a224e30d8a70418541a6185ce5765b37745a7a (patch) | |
tree | 938bd443fc9f53cdbea3294d2e702e72ddd8493c /src/modules/uavcan/uavcan_main.cpp | |
parent | 821c06f7cc58b50afe80442ee2258bf99cbe1fd2 (diff) | |
parent | 685d3965a81b3f6a1ada4aa8bf9ebdd16b029c58 (diff) | |
download | px4-firmware-f3a224e30d8a70418541a6185ce5765b37745a7a.tar.gz px4-firmware-f3a224e30d8a70418541a6185ce5765b37745a7a.tar.bz2 px4-firmware-f3a224e30d8a70418541a6185ce5765b37745a7a.zip |
Merged master
Diffstat (limited to 'src/modules/uavcan/uavcan_main.cpp')
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 29 |
1 files changed, 24 insertions, 5 deletions
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..2c543462e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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,13 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + 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); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +391,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); } @@ -429,7 +448,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } |