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

UAVCAN: implemented motor testing

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/uavcan/actuators/esc.hpp
View File

@ -103,7 +103,6 @@ private:
* ESC states
*/
uint32_t _armed_mask = 0;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters

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);
}

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

@ -41,6 +41,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/test_motor.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
@ -103,6 +104,10 @@ private:
actuator_armed_s _armed; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
test_motor_s _test_motor;
bool _test_in_progress = false;
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer