fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

tap_esc: subscribe to test_motor topic and set outputs accordingly in non-armed state

Browse Source 
This makes it possible to use the 'motor_test' command
This commit is contained in:
Beat Küng 2016-07-19 12:49:52 +02:00
parent 0f6a6f7150
commit 38e4882b5f
1 changed files with 36 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/drivers/tap_esc/tap_esc.cpp
View File

@ -44,6 +44,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 <uORB/topics/input_rc.h>
#include <uORB/topics/esc_status.h>
@ -98,6 +99,7 @@ private:
// subscriptions
unsigned _num_outputs;
int _armed_sub;
int _test_motor_sub;
orb_advert_t _outputs_pub = nullptr;
actuator_outputs_s _outputs;
static actuator_armed_s _armed;
@ -152,6 +154,7 @@ TAP_ESC::TAP_ESC():
_mode(MODE_4PWM),
_num_outputs(0),
_armed_sub(-1),
_test_motor_sub(-1),
_outputs_pub(nullptr),
_control_subs{ -1},
_esc_feedback_pub(nullptr),
@ -437,6 +440,7 @@ TAP_ESC::cycle()
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
_esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback);
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
_initialized = true;
}
@ -477,19 +481,14 @@ TAP_ESC::cycle()
/* check if anything updated */
int ret = ::poll(_poll_fds, _poll_fds_num, 0);
int ret = ::poll(_poll_fds, _poll_fds_num, 5);
/* this would be bad... */
if (ret < 0) {
DEVICE_LOG("poll error %d", errno);
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
//printf("timeout\n");
} else {
} else { /* update even in the case of a timeout, to check for test_motor commands */
/* get controls for required topics */
unsigned poll_id = 0;
@ -539,10 +538,8 @@ TAP_ESC::cycle()
_outputs.timestamp = hrt_absolute_time();
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
if (i >= num_outputs) {
_outputs.output[i] = NAN;
}
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
/* iterate actuators */
@ -570,18 +567,30 @@ TAP_ESC::cycle()
_outputs.noutputs = num_outputs;
_outputs.timestamp = hrt_absolute_time();
for (unsigned i = 0; i < num_outputs; i++) {
_outputs.output[i] = 900;
/* check for motor test commands */
bool test_motor_updated;
orb_check(_test_motor_sub, &test_motor_updated);
if (test_motor_updated) {
struct test_motor_s test_motor;
orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
_outputs.output[test_motor.motor_number] = 900.f + (1100.f * test_motor.value); //scale to [900, 2000]
PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
(double)_outputs.output[test_motor.motor_number]);
}
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
if (i >= num_outputs) {
_outputs.output[i] = NAN;
/* set the invalid values to the minimum */
for (unsigned i = 0; i < num_outputs; i++) {
if (!PX4_ISFINITE(_outputs.output[i])) {
_outputs.output[i] = 900;
}
}
/* disable unused ports by setting their output to NaN */
for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
}
const unsigned esc_count = 4;
@ -630,6 +639,11 @@ TAP_ESC::cycle()
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
/* do not obey the lockdown value, as lockdown is for PWMSim */
IS_armed = _armed.armed;
/* reset all outputs */
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
}
@ -639,12 +653,15 @@ void TAP_ESC::work_stop()
{
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1;
}
}
::close(_armed_sub);
orb_unsubscribe(_armed_sub);
_armed_sub = -1;
orb_unsubscribe(_test_motor_sub);
_test_motor_sub = -1;
DEVICE_LOG("stopping");
_initialized = false;