diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 12f6c86372..5b669f2a78 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -61,10 +61,6 @@ #include "tap_esc_common.h" -#ifndef B250000 -#define B250000 250000 -#endif - #include "drv_tap_esc.h" #if !defined(BOARD_TAP_ESC_NO_VERIFY_CONFIG) @@ -153,11 +149,7 @@ private: void work_start(); void work_stop(); void send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm); - uint8_t crc8_esc(uint8_t *p, uint8_t len); - uint8_t crc_packet(EscPacket &p); - 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_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); @@ -277,7 +269,7 @@ TAP_ESC::init() config.maxChannelValue = RPMMAX; config.minChannelValue = RPMMIN; - ret = send_packet(packet, 0); + ret = tap_esc_common::send_packet(_uart_fd, packet, 0); if (ret < 0) { return ret; @@ -295,7 +287,7 @@ TAP_ESC::init() info_req.channelID = cid; info_req.requestInfoType = REQEST_INFO_BASIC; - ret = send_packet(packet_info, cid); + ret = tap_esc_common::send_packet(_uart_fd, packet_info, cid); if (ret < 0) { return ret; @@ -308,9 +300,9 @@ TAP_ESC::init() while (retries--) { - read_data_from_uart(); + tap_esc_common::read_data_from_uart(_uart_fd, &uartbuf); - if (parse_tap_esc_feedback(&uartbuf, &_packet)) { + if (tap_esc_common::parse_tap_esc_feedback(&uartbuf, &_packet)) { valid = (_packet.msg_id == ESCBUS_MSG_ID_CONFIG_INFO_BASIC && _packet.d.rspConfigInfoBasic.channelID == cid && 0 == memcmp(&_packet.d.rspConfigInfoBasic.resp, &config, sizeof(ConfigInfoBasicRequest))); @@ -344,7 +336,7 @@ TAP_ESC::init() while (unlock_times--) { - send_packet(unlock_packet, -1); + tap_esc_common::send_packet(_uart_fd, unlock_packet, -1); /* Min Packet to Packet time is 1 Ms so use 2 */ @@ -358,27 +350,6 @@ TAP_ESC::init() return ret; } -int TAP_ESC::send_packet(EscPacket &packet, int responder) -{ - if (responder >= 0) { - - if (responder > _channels_count) { - return -EINVAL; - } - - tap_esc_common::select_responder(responder); - } - - int packet_len = crc_packet(packet); - int ret = ::write(_uart_fd, &packet.head, packet_len); - - if (ret != packet_len) { - PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); - } - - return ret; -} - void TAP_ESC::subscribe() { @@ -407,24 +378,6 @@ TAP_ESC::subscribe() } } -uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len) -{ - uint8_t crc = 0; - - for (uint8_t i = 0; i < len; i++) { - crc = tap_esc_common::crc_table[crc^*p++]; - } - - return crc; -} - -uint8_t TAP_ESC::crc_packet(EscPacket &p) -{ - /* Calculate the crc over Len,ID,data */ - p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2); - return p.len + offsetof(EscPacket, d) + 1; -} - void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm) { @@ -454,7 +407,7 @@ void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm) packet.d.reqRun.rpm_flags[i] = rpm[i]; } - int ret = send_packet(packet, which_to_respone); + int ret = tap_esc_common::send_packet(_uart_fd, packet, which_to_respone); if (++which_to_respone == _channels_count) { which_to_respone = 0; @@ -465,113 +418,6 @@ void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm) } } -void TAP_ESC::read_data_from_uart() -{ - uint8_t tmp_serial_buf[UART_BUFFER_SIZE]; - - int len =::read(_uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf)); - - if (len > 0 && (uartbuf.dat_cnt + len < UART_BUFFER_SIZE)) { - for (int i = 0; i < len; i++) { - uartbuf.esc_feedback_buf[uartbuf.tail++] = tmp_serial_buf[i]; - uartbuf.dat_cnt++; - - if (uartbuf.tail >= UART_BUFFER_SIZE) { - uartbuf.tail = 0; - } - } - } -} - -bool TAP_ESC::parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata) -{ - static PARSR_ESC_STATE state = HEAD; - static uint8_t data_index = 0; - static uint8_t crc_data_cal; - - if (serial_buf->dat_cnt > 0) { - int count = serial_buf->dat_cnt; - - for (int i = 0; i < count; i++) { - switch (state) { - case HEAD: - if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) { - packetdata->head = 0xFE; //just_keep the format - state = LEN; - } - - break; - - case LEN: - if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) { - packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head]; - state = ID; - - } else { - state = HEAD; - } - - break; - - case ID: - if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) { - packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head]; - data_index = 0; - state = DATA; - - } else { - state = HEAD; - } - - break; - - case DATA: - packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head]; - - if (data_index >= packetdata->len) { - - crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2); - state = CRC; - } - - break; - - case CRC: - if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) { - packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head]; - - if (++serial_buf->head >= UART_BUFFER_SIZE) { - serial_buf->head = 0; - } - - serial_buf->dat_cnt--; - state = HEAD; - return true; - } - - state = HEAD; - break; - - default: - state = HEAD; - break; - - } - - if (++serial_buf->head >= UART_BUFFER_SIZE) { - serial_buf->head = 0; - } - - serial_buf->dat_cnt--; - - } - - - } - - return false; -} - void TAP_ESC::cycle() { @@ -752,9 +598,9 @@ TAP_ESC::cycle() } send_esc_outputs(motor_out, esc_count); - read_data_from_uart(); + tap_esc_common::read_data_from_uart(_uart_fd, &uartbuf); - if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) { + if (tap_esc_common::parse_tap_esc_feedback(&uartbuf, &_packet) == true) { if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) { RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo; @@ -935,8 +781,6 @@ TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) namespace tap_esc_drv { - - volatile bool _task_should_exit = false; // flag indicating if tap_esc task should exit static int _uart_fd = -1; static char _device[32] = {}; @@ -944,8 +788,6 @@ static bool _is_running = false; // flag indicating if tap_esc app is ru static px4_task_t _task_handle = -1; // handle to the task main thread static int _supported_channel_count = 0; -static bool _flow_control_enabled = false; - void usage(); void start(); @@ -957,12 +799,6 @@ void task_main_trampoline(int argc, char *argv[]); void task_main(int argc, char *argv[]); -int initialise_uart(); - -int deinitialize_uart(); - -int enable_flow_control(bool enabled); - int tap_esc_start(void) { int ret = OK; @@ -1001,79 +837,13 @@ int tap_esc_stop(void) return ret; } -int initialise_uart() -{ - // open uart - _uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK); - int termios_state = -1; - - if (_uart_fd < 0) { - PX4_ERR("failed to open uart device!"); - return -1; - } - - // set baud rate - int speed = B250000; - struct termios uart_config; - tcgetattr(_uart_fd, &uart_config); - - // clear ONLCR flag (which appends a CR for every LF) - uart_config.c_oflag &= ~ONLCR; - - // set baud rate - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state); - close(_uart_fd); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - PX4_ERR("tcsetattr failed for %s\n", _device); - close(_uart_fd); - return -1; - } - - // setup output flow control - if (enable_flow_control(false)) { - PX4_WARN("hardware flow disable failed"); - } - - return _uart_fd; -} - -int enable_flow_control(bool enabled) -{ - struct termios uart_config; - - int ret = tcgetattr(_uart_fd, &uart_config); - - if (enabled) { - uart_config.c_cflag |= CRTSCTS; - - } else { - uart_config.c_cflag &= ~CRTSCTS; - } - - ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); - - if (!ret) { - _flow_control_enabled = enabled; - } - - return ret; -} - -int deinitialize_uart() -{ - return close(_uart_fd); -} void task_main(int argc, char *argv[]) { _is_running = true; - if (initialise_uart() < 0) { + if (tap_esc_common::initialise_uart(_device, _uart_fd) < 0) { PX4_ERR("Failed to initialize UART."); while (_task_should_exit == false) { @@ -1138,7 +908,7 @@ void stop() } tap_esc_stop(); - deinitialize_uart(); + tap_esc_common::deinitialise_uart(_uart_fd); _task_handle = -1; } diff --git a/src/drivers/tap_esc/tap_esc_common.cpp b/src/drivers/tap_esc/tap_esc_common.cpp index 962fa9939c..a5837beca7 100644 --- a/src/drivers/tap_esc/tap_esc_common.cpp +++ b/src/drivers/tap_esc/tap_esc_common.cpp @@ -36,12 +36,24 @@ * */ -#include #include "tap_esc_common.h" +#include +#include + +#include // arraySize +#include +#include + +#ifndef B250000 +#define B250000 250000 +#endif namespace tap_esc_common { +static uint8_t crc8_esc(uint8_t *p, uint8_t len); +static uint8_t crc_packet(EscPacket &p); + /**************************************************************************** * Name: select_responder * @@ -59,6 +71,214 @@ void select_responder(uint8_t sel) #endif } +int initialise_uart(const char *const device, int &uart_fd) +{ + // open uart + uart_fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + int termios_state = -1; + + if (uart_fd < 0) { + PX4_ERR("failed to open uart device!"); + return -1; + } + + // set baud rate + int speed = B250000; + struct termios uart_config; + tcgetattr(uart_fd, &uart_config); + + // clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; + + // set baud rate + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + PX4_ERR("failed to set baudrate for %s: %d\n", device, termios_state); + close(uart_fd); + return -1; + } + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("tcsetattr failed for %s\n", device); + close(uart_fd); + return -1; + } + + // setup output flow control + if (enable_flow_control(uart_fd, false)) { + PX4_WARN("hardware flow disable failed"); + } + + return uart_fd; +} + +int deinitialise_uart(int &uart_fd) +{ + return close(uart_fd); +} + +int enable_flow_control(int uart_fd, bool enabled) +{ + struct termios uart_config; + + int ret = tcgetattr(uart_fd, &uart_config); + + if (ret != 0) { + PX4_ERR("error getting uart configuration"); + return ret; + } + + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + + return tcsetattr(uart_fd, TCSANOW, &uart_config); +} + +int send_packet(int uart_fd, EscPacket &packet, int responder) +{ + if (responder >= 0) { + select_responder(responder); + } + + int packet_len = crc_packet(packet); + int ret = ::write(uart_fd, &packet.head, packet_len); + + if (ret != packet_len) { + PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); + } + + return ret; +} + +void read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf) +{ + uint8_t tmp_serial_buf[UART_BUFFER_SIZE]; + + int len =::read(uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf)); + + if (len > 0 && (uart_buf->dat_cnt + len < UART_BUFFER_SIZE)) { + for (int i = 0; i < len; i++) { + uart_buf->esc_feedback_buf[uart_buf->tail++] = tmp_serial_buf[i]; + uart_buf->dat_cnt++; + + if (uart_buf->tail >= UART_BUFFER_SIZE) { + uart_buf->tail = 0; + } + } + } +} + +bool parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata) +{ + static PARSR_ESC_STATE state = HEAD; + static uint8_t data_index = 0; + static uint8_t crc_data_cal; + + if (serial_buf->dat_cnt > 0) { + int count = serial_buf->dat_cnt; + + for (int i = 0; i < count; i++) { + switch (state) { + case HEAD: + if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) { + packetdata->head = 0xFE; //just_keep the format + state = LEN; + } + + break; + + case LEN: + if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) { + packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head]; + state = ID; + + } else { + state = HEAD; + } + + break; + + case ID: + if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) { + packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head]; + data_index = 0; + state = DATA; + + } else { + state = HEAD; + } + + break; + + case DATA: + packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head]; + + if (data_index >= packetdata->len) { + + crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2); + state = CRC; + } + + break; + + case CRC: + if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) { + packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head]; + + if (++serial_buf->head >= UART_BUFFER_SIZE) { + serial_buf->head = 0; + } + + serial_buf->dat_cnt--; + state = HEAD; + return true; + } + + state = HEAD; + break; + + default: + state = HEAD; + break; + + } + + if (++serial_buf->head >= UART_BUFFER_SIZE) { + serial_buf->head = 0; + } + + serial_buf->dat_cnt--; + + } + + + } + + return false; +} + +static uint8_t crc8_esc(uint8_t *p, uint8_t len) +{ + uint8_t crc = 0; + + for (uint8_t i = 0; i < len; i++) { + crc = crc_table[crc^*p++]; + } + + return crc; +} + +static uint8_t crc_packet(EscPacket &p) +{ + /* Calculate the crc over Len,ID,data */ + p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2); + return p.len + offsetof(EscPacket, d) + 1; +} + + const uint8_t crc_table[256] = { 0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A, 0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33, diff --git a/src/drivers/tap_esc/tap_esc_common.h b/src/drivers/tap_esc/tap_esc_common.h index bfa9e5100c..195dcc6959 100644 --- a/src/drivers/tap_esc/tap_esc_common.h +++ b/src/drivers/tap_esc/tap_esc_common.h @@ -40,6 +40,9 @@ #include +#include "drv_tap_esc.h" + + namespace tap_esc_common { /**************************************************************************** @@ -51,5 +54,17 @@ namespace tap_esc_common ****************************************************************************/ void select_responder(uint8_t sel); +int initialise_uart(const char *const device, int &uart_fd); + +int deinitialise_uart(int &uart_fd); + +int enable_flow_control(int uart_fd, bool enabled); + +int send_packet(int uart_fd, EscPacket &packet, int responder); + +void read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf); + +bool parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata); + extern const uint8_t crc_table[]; } /* tap_esc_common */