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

tap_esc: moved common functions to tap_esc_common

Browse Source
This commit is contained in:
Alessandro Simovic 2018-04-11 14:54:35 +02:00 committed by Beat Küng
parent 4ab8c2a682
commit f95b823e31
3 changed files with 247 additions and 242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
}

222
src/drivers/tap_esc/tap_esc_common.cpp
View File

@ -36,12 +36,24 @@
*
*/
#include <px4_config.h>
#include "tap_esc_common.h"
#include <fcntl.h>
#include <termios.h>
#include <systemlib/px4_macros.h> // arraySize
#include <px4_config.h>
#include <px4_defines.h>
#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,

15
src/drivers/tap_esc/tap_esc_common.h
View File

@ -40,6 +40,9 @@
#include <stdint.h>
#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 */