53 #ifdef HAL_RCOUTPUT_TAP_DEVICE 61 #include <sys/types.h> 69 #define debug(fmt, args...) ::printf(fmt "\n", ##args) 71 #define debug(fmt, args...) 78 #define ESC_HAVE_CURRENT_SENSOR 80 static const uint8_t crcTable[256] = {
81 0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
82 0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,
83 0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,
84 0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,
85 0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,
86 0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,
87 0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,
88 0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,
89 0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,
90 0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,
91 0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,
92 0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,
93 0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,
94 0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,
95 0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,
96 0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,
97 0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,
98 0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,
99 0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,
100 0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,
101 0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,
102 0xB8, 0x5F, 0x91, 0x76
106 static const uint8_t device_mux_map[] = {0, 1, 4, 3, 2, 5, 7, 8};
108 static const uint8_t device_dir_map[] = {0, 1, 0, 1, 0, 1, 0, 1};
110 #define TAP_ESC_MAX_PACKET_LEN 20 111 #define TAP_ESC_MAX_MOTOR_NUM 8 132 #define RPMSTOPPED (RPMMIN - 10) 134 #define MIN_BOOT_TIME_MSEC (550) // Minimum time to wait after Power on before sending commands 140 #define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff 141 #define RUN_RED_LED_ON_MASK (uint16_t)0x0800 142 #define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000 143 #define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000 144 #define RUN_LED_ON_MASK (uint16_t)0x3800 145 #define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000 146 #define RUN_REVERSE_MASK (uint16_t)0x8000 149 uint16_t
value[TAP_ESC_MAX_MOTOR_NUM];
152 struct PACKED RunInfoRepsonse {
156 #if defined(ESC_HAVE_VOLTAGE_SENSOR) 159 #if defined(ESC_HAVE_CURRENT_SENSOR) 162 #if defined(ESC_HAVE_TEMPERATURE_SENSOR) 169 struct PACKED ConfigInfoBasicRequest {
170 uint8_t maxChannelInUse;
171 uint8_t channelMapTable[TAP_ESC_MAX_MOTOR_NUM];
172 uint8_t monitorMsgType;
174 uint16_t minChannelValue;
175 uint16_t maxChannelValue;
178 struct PACKED ConfigInfoBasicResponse {
180 ConfigInfoBasicRequest resp;
183 #define ESC_CHANNEL_MAP_CHANNEL 0x0f 184 #define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0 189 REQUEST_INFO_BASIC = 0,
197 struct PACKED InfoRequest {
199 uint8_t requestInfoType;
210 ConfigInfoBasicRequest reqConfigInfoBasic;
213 ConfigInfoBasicResponse rspConfigInfoBasic;
214 RunInfoRepsonse rspRunInfo;
221 static const unsigned ESC_PACKET_DATA_OFFSET = 3;
249 enum ESCBUS_ENUM_ESC_STATUS {
251 ESC_STATUS_WARNING_LOW_VOLTAGE,
252 ESC_STATUS_WARNING_OVER_HEAT,
253 ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP,
254 ESC_STATUS_ERROR_MOTOR_STALL,
255 ESC_STATUS_ERROR_HARDWARE,
256 ESC_STATUS_ERROR_LOSE_PROPELLER,
257 ESC_STATUS_ERROR_OVER_CURRENT,
258 ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP,
259 ESC_STATUS_ERROR_LOSE_CMD,
262 enum ESCBUS_ENUM_MESSAGE_ID {
264 ESCBUS_MSG_ID_CONFIG_BASIC = 0,
265 ESCBUS_MSG_ID_CONFIG_FULL,
268 ESCBUS_MSG_ID_DO_CMD,
270 ESCBUS_MSG_ID_REQUEST_INFO,
271 ESCBUS_MSG_ID_CONFIG_INFO_BASIC,
272 ESCBUS_MSG_ID_CONFIG_INFO_FULL,
273 ESCBUS_MSG_ID_RUN_INFO,
274 ESCBUS_MSG_ID_STUDY_INFO,
275 ESCBUS_MSG_ID_COMM_INFO,
276 ESCBUS_MSG_ID_DEVICE_INFO,
277 ESCBUS_MSG_ID_ASSIGNED_ID,
282 ESCBUS_MSG_ID_BOOT_SYNC = 0x21,
283 PROTO_GET_DEVICE = 0x22,
284 PROTO_CHIP_ERASE = 0x23,
285 PROTO_PROG_MULTI = 0x27,
286 PROTO_GET_CRC = 0x29,
288 PROTO_GET_SOFTWARE_VERSION = 0x40,
289 ESCBUS_MSG_ID_MAX_NUM,
292 enum PARSR_ESC_STATE {
317 _uart_fd =
open(HAL_RCOUTPUT_TAP_DEVICE, O_RDWR | O_NOCTTY | O_NONBLOCK);
318 int termios_state = -1;
321 ::fprintf(stderr,
"failed to open uart device! %s\n", HAL_RCOUTPUT_TAP_DEVICE);
325 struct termios uart_config;
326 memset(&uart_config, 0,
sizeof(uart_config));
327 tcgetattr(_uart_fd, &uart_config);
330 uart_config.c_oflag &= ~ONLCR;
332 if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
333 ::fprintf(stderr,
"tcsetattr failed for %s\n", HAL_RCOUTPUT_TAP_DEVICE);
338 if (!_uart_set_speed(250000)) {
339 ::fprintf(stderr,
"failed to set baudrate for %s: %m\n",
340 HAL_RCOUTPUT_TAP_DEVICE);
358 if (now < MIN_BOOT_TIME_MSEC) {
364 EscPacket packet = {0xfe,
sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC};
365 ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic;
366 memset(&config, 0,
sizeof(ConfigInfoBasicRequest));
367 config.maxChannelInUse = _channels_count;
368 config.controlMode = 1;
371 for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) {
372 config.channelMapTable[phy_chan_index] = device_mux_map[phy_chan_index] & ESC_CHANNEL_MAP_CHANNEL;
373 config.channelMapTable[phy_chan_index] |= (device_dir_map[phy_chan_index] << 4) & ESC_CHANNEL_MAP_RUNNING_DIRECTION;
376 config.maxChannelValue = RPMMAX;
377 config.minChannelValue = RPMMIN;
379 int ret = _send_packet(packet);
382 AP_HAL::panic(
"Unable to send configuration to " HAL_RCOUTPUT_TAP_DEVICE);
390 EscPacket unlock_packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
391 unlock_packet.len *=
sizeof(unlock_packet.d.reqRun.value[0]);
392 memset(unlock_packet.d.bytes, 0,
sizeof(packet.d.bytes));
394 for (uint8_t i = 0; i < 10; i++) {
395 _send_packet(unlock_packet);
403 int packet_len = _crc_packet(packet);
404 int ret =
::write(_uart_fd, &packet.head, packet_len);
406 if (ret != packet_len) {
407 debug(
"TX ERROR: ret: %d, errno: %d", ret,
errno);
417 for (uint8_t i = 0; i < len; i++) {
418 crc = crcTable[crc ^ *p++];
427 p.d.bytes[p.len] = _crc8_esc(&p.len, p.len + 2);
428 return p.len + ESC_PACKET_DATA_OFFSET + 1;
445 if (ch >= MAX_MOTORS) {
448 _enabled_channels |= (1U << ch);
453 if (ch >= MAX_MOTORS) {
457 _enabled_channels &= ~(1U << ch);
462 if (ch >= MAX_MOTORS) {
465 if (!(_enabled_channels & (1U << ch))) {
470 _period[ch] = period_us;
479 if (ch >= MAX_MOTORS) {
487 for (uint8_t i = 0; i < len; i++) {
488 period_us[i] =
read(i);
503 uint16_t out[TAP_ESC_MAX_MOTOR_NUM];
504 uint8_t motor_cnt = _channels_count;
506 uint8_t motor_mapping[] = {
514 for (uint8_t i = 0; i < motor_cnt; i++) {
515 uint16_t *val = &out[motor_mapping[i]];
517 if (!(_enabled_channels & (1U << i))) {
519 }
else if (_period[i] < _esc_pwm_min) {
521 }
else if (_period[i] >= _esc_pwm_max) {
524 float period_us =
constrain_int16(_period[i], _esc_pwm_min, _esc_pwm_max);
531 float rpm = (period_us - _esc_pwm_min)/(_esc_pwm_max - _esc_pwm_min)
532 * (RPMMAX - RPMSTOPPED) + RPMSTOPPED;
534 *val = (uint16_t) rpm;
537 for (uint8_t i = motor_cnt; i < TAP_ESC_MAX_MOTOR_NUM; i++) {
556 EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
557 packet.len *=
sizeof(packet.d.reqRun.value[0]);
560 if (tnow - _last_led_update_msec > 250) {
562 _last_led_update_msec = tnow;
565 for (uint8_t i = 0; i < _channels_count; i++) {
566 packet.d.reqRun.value[i] = out[i] & RUN_CHANNEL_VALUE_MASK;
568 packet.d.reqRun.value[i] |= RUN_LED_ON_MASK;
572 int ret = _send_packet(packet);
574 debug(
"TX ERROR: ret: %d, errno: %d", ret,
errno);
static uint8_t _crc8_esc(uint8_t *p, uint8_t len)
virtual perf_counter_t perf_alloc(perf_counter_type t, const char *name)
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
uint16_t read(uint8_t ch) override
ssize_t write(int fd, const void *buf, size_t count)
POSIX Write count bytes from *buf to fileno fd.
int _send_packet(EscPacket &p)
#define debug(fmt, args ...)
virtual void delay(uint16_t ms)=0
void set_freq(uint32_t chmask, uint16_t freq_hz) override
void write(uint8_t ch, uint16_t period_us) override
ssize_t read(int fd, void *buf, size_t count)
POSIX read count bytes from *buf to fileno fd.
int close(int fileno)
POSIX Close a file with fileno handel.
virtual void perf_end(perf_counter_t h)
virtual void perf_begin(perf_counter_t h)
uint16_t get_freq(uint8_t ch) override
int errno
Note: fdevopen assigns stdin,stdout,stderr.
int fprintf(FILE *fp, const char *fmt,...)
fprintf character write function
static uint8_t _crc_packet(EscPacket &p)
void disable_ch(uint8_t ch) override
void panic(const char *errormsg,...) FMT_PRINTF(1
void enable_ch(uint8_t ch) override
AP_HAL::Scheduler * scheduler