fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 11:33:33 +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 11:46:09 +02:00
committed by Lorenz Meier
parent 7aea4a33c0
commit 96c40d3ce9
1 changed files with 23 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/tap_esc/tap_esc.cpp
+23 -22
View File
@@ -109,12 +109,12 @@ private:
// subscriptions // subscriptions
int _armed_sub; int _armed_sub;
int _test_motor_sub; int _test_motor_sub;
orb_advert_t _outputs_pub = nullptr; orb_advert_t _outputs_pub;
actuator_outputs_s _outputs; actuator_outputs_s _outputs;
static actuator_armed_s _armed; actuator_armed_s _armed;
//todo:refactor dynamic based on _channels_count //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]; int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[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); int send_packet(EscPacket &p, int responder);
void read_data_from_uart(); void read_data_from_uart();
bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata); bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata);
static int control_callback(uintptr_t handle, static int control_callback_trampoline(uintptr_t handle,
uint8_t control_group, uint8_t control_group, uint8_t control_index, float &input);
uint8_t control_index, inline int control_callback(uint8_t control_group, uint8_t control_index, float &input);
float &input);
}; };
const uint8_t TAP_ESC::crcTable[256] = TAP_ESC_CRC; 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_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS;
const uint8_t TAP_ESC::device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR; const uint8_t TAP_ESC::device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR;
actuator_armed_s TAP_ESC::_armed = {};
namespace namespace
{ {
TAP_ESC *tap_esc = nullptr; TAP_ESC *tap_esc = nullptr;
@@ -173,7 +170,7 @@ TAP_ESC::TAP_ESC(int channels_count):
_armed_sub(-1), _armed_sub(-1),
_test_motor_sub(-1), _test_motor_sub(-1),
_outputs_pub(nullptr), _outputs_pub(nullptr),
_control_subs{ -1}, _armed{},
_esc_feedback_pub(nullptr), _esc_feedback_pub(nullptr),
_to_mixer_status(nullptr), _to_mixer_status(nullptr),
_esc_feedback{}, _esc_feedback{},
@@ -197,6 +194,10 @@ TAP_ESC::TAP_ESC(int channels_count):
uartbuf.dat_cnt = 0; uartbuf.dat_cnt = 0;
memset(uartbuf.esc_feedback_buf, 0, sizeof(uartbuf.esc_feedback_buf)); 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++) { for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN; _outputs.output[i] = NAN;
} }
@@ -382,7 +383,7 @@ TAP_ESC::subscribe()
_control_subs[i] = -1; _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].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN; _poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++; _poll_fds_num++;
@@ -605,7 +606,7 @@ TAP_ESC::cycle()
DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); 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++) { 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); orb_set_interval(_control_subs[i], update_rate_in_ms);
} }
} }
@@ -630,7 +631,7 @@ TAP_ESC::cycle()
unsigned poll_id = 0; unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 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) { if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -797,7 +798,7 @@ TAP_ESC::cycle()
void TAP_ESC::work_stop() void TAP_ESC::work_stop()
{ {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 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]); orb_unsubscribe(_control_subs[i]);
_control_subs[i] = -1; _control_subs[i] = -1;
} }
@@ -812,15 +813,15 @@ void TAP_ESC::work_stop()
_initialized = false; _initialized = false;
} }
int int TAP_ESC::control_callback_trampoline(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
TAP_ESC::control_callback(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 */ /* limit control input */
if (input > 1.0f) { if (input > 1.0f) {
@@ -876,7 +877,7 @@ TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strnlen(buf, 1024); unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr) { if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls); _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
} }
if (_mixers == nullptr) { if (_mixers == nullptr) {