forked from Archive/PX4-Autopilot
UAVCAN: implemented motor testing
This commit is contained in:
parent
58f36714f8
commit
0800fa4715
|
@ -103,7 +103,6 @@ private:
|
||||||
* ESC states
|
* ESC states
|
||||||
*/
|
*/
|
||||||
uint32_t _armed_mask = 0;
|
uint32_t _armed_mask = 0;
|
||||||
uavcan::equipment::esc::Status _states[MAX_ESCS];
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Perf counters
|
* Perf counters
|
||||||
|
|
|
@ -279,6 +279,7 @@ int UavcanNode::run()
|
||||||
_output_count = 2;
|
_output_count = 2;
|
||||||
|
|
||||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
|
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
|
||||||
|
|
||||||
actuator_outputs_s outputs;
|
actuator_outputs_s outputs;
|
||||||
memset(&outputs, 0, sizeof(outputs));
|
memset(&outputs, 0, sizeof(outputs));
|
||||||
|
@ -344,7 +345,14 @@ int UavcanNode::run()
|
||||||
}
|
}
|
||||||
|
|
||||||
// can we mix?
|
// 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,
|
// XXX one output group has 8 outputs max,
|
||||||
// but this driver could well serve multiple groups.
|
// 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;
|
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);
|
orb_check(_armed_sub, &updated);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
||||||
|
|
||||||
// Update the armed status and check that we're not locked down
|
// Update the armed status and check that we're not locked down and motor
|
||||||
bool set_armed = _armed.armed && !_armed.lockdown;
|
// test is not running
|
||||||
|
bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
|
||||||
|
|
||||||
arm_actuators(set_armed);
|
arm_actuators(set_armed);
|
||||||
}
|
}
|
||||||
|
|
|
@ -41,6 +41,7 @@
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
#include <uORB/topics/actuator_armed.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
|
#include <uORB/topics/test_motor.h>
|
||||||
|
|
||||||
#include "actuators/esc.hpp"
|
#include "actuators/esc.hpp"
|
||||||
#include "sensors/sensor_bridge.hpp"
|
#include "sensors/sensor_bridge.hpp"
|
||||||
|
@ -103,6 +104,10 @@ private:
|
||||||
actuator_armed_s _armed; ///< the arming request of the system
|
actuator_armed_s _armed; ///< the arming request of the system
|
||||||
bool _is_armed = false; ///< the arming status of the actuators on the bus
|
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
|
unsigned _output_count = 0; ///< number of actuators currently available
|
||||||
|
|
||||||
static UavcanNode *_instance; ///< singleton pointer
|
static UavcanNode *_instance; ///< singleton pointer
|
||||||
|
|
Loading…
Reference in New Issue