fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 12:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

tap_esc: improve cleanup & initialization code

Browse Source
This commit is contained in:
Beat Küng 2017-04-06 10:39:17 +02:00 committed by Lorenz Meier
parent 7aea4a33c0
commit 96c40d3ce9
1 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -109,12 +109,12 @@ private:
// subscriptions
int _armed_sub;
int _test_motor_sub;
orb_advert_t _outputs_pub = nullptr;
orb_advert_t _outputs_pub;
actuator_outputs_s _outputs;
static actuator_armed_s _armed;
actuator_armed_s _armed;
//todo:refactor dynamic based on _channels_count
// It needs to support the numbe of ESC
// It needs to support the number of ESC
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
@ -146,18 +146,15 @@ private:
int send_packet(EscPacket &p, int responder);
void read_data_from_uart();
bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata);
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
static int control_callback_trampoline(uintptr_t handle,
uint8_t control_group, uint8_t control_index, float &input);
inline int control_callback(uint8_t control_group, uint8_t control_index, float &input);
};
const uint8_t TAP_ESC::crcTable[256] = TAP_ESC_CRC;
const uint8_t TAP_ESC::device_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS;
const uint8_t TAP_ESC::device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR;
actuator_armed_s TAP_ESC::_armed = {};
namespace
{
TAP_ESC *tap_esc = nullptr;
@ -173,7 +170,7 @@ TAP_ESC::TAP_ESC(int channels_count):
_armed_sub(-1),
_test_motor_sub(-1),
_outputs_pub(nullptr),
_control_subs{ -1},
_armed{},
_esc_feedback_pub(nullptr),
_to_mixer_status(nullptr),
_esc_feedback{},
@ -197,6 +194,10 @@ TAP_ESC::TAP_ESC(int channels_count):
uartbuf.dat_cnt = 0;
memset(uartbuf.esc_feedback_buf, 0, sizeof(uartbuf.esc_feedback_buf));
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {
_control_subs[i] = -1;
}
for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
@ -382,7 +383,7 @@ TAP_ESC::subscribe()
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
if (_control_subs[i] >= 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
@ -605,7 +606,7 @@ TAP_ESC::cycle()
DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_control_subs[i] >= 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
@ -630,7 +631,7 @@ TAP_ESC::cycle()
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_control_subs[i] >= 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@ -797,7 +798,7 @@ TAP_ESC::cycle()
void TAP_ESC::work_stop()
{
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_control_subs[i] >= 0) {
orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1;
}
@ -812,15 +813,15 @@ void TAP_ESC::work_stop()
_initialized = false;
}
int
TAP_ESC::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
int TAP_ESC::control_callback_trampoline(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
TAP_ESC *obj = (TAP_ESC *)handle;
return obj->control_callback(control_group, control_index, input);
}
input = controls[control_group].control[control_index];
int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, float &input)
{
input = _controls[control_group].control[control_index];
/* limit control input */
if (input > 1.0f) {
@ -876,7 +877,7 @@ TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
}
if (_mixers == nullptr) {