From 632e4630b17c5c13a6f132956f137e6d794a5924 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 Jul 2016 14:49:03 -1000 Subject: [PATCH] TAP:ESC motors and LED working - The code is still hardcoded for 4 ESC But the basics are working. N.B. To Stop the moros a value of 0 need to be sent. LED color mask are misleeding - LED is On or Off per ESC. --- src/drivers/tap_esc/drv_tap_esc.h | 124 +++++++++++--- src/drivers/tap_esc/tap_esc.cpp | 268 +++++++++++++++++++++--------- 2 files changed, 290 insertions(+), 102 deletions(-) diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h index 01cc7e078e..a84cdee4ac 100644 --- a/src/drivers/tap_esc/drv_tap_esc.h +++ b/src/drivers/tap_esc/drv_tap_esc.h @@ -59,49 +59,121 @@ #define TAP_ESC_MAX_PACKET_LEN 20 #define TAP_ESC_MAX_MOTOR_NUM 8 +/* ESC_POS maps the values stored in the channelMapTable to reorder the ESC's + * id so that that match the mux setting, so that the ressonder's data + * will be read. + * The index on channelMapTable[p] p is the physical ESC + * The value it is set to is the logical value from ESC_POS[p] + * Phy Log + * 0 0 + * 1 1 + * 2 4 + * 3 3 + * 4 2 + * 5 5 + * .... + * + */ +#define ESC_POS {0, 1, 4, 3, 2, 5, 7, 8} + #define RPMMAX 1900 -#define RPMMIN 1100 +#define RPMMIN 1200 -#define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff -#define RUN_RED_LED_ON_MASK (uint16_t)0x0800 -#define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000 -#define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000 -#define RUN_LED_ON_MASK (uint16_t)0x3800 -#define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000 -#define RUN_REVERSE_MASK (uint16_t)0x8000 +#define MAX_BOOT_TIME_MS (500) // Minimum time to wait after Power on before sending commands #pragma pack(push,1) -typedef struct { - uint8_t ESC_ID; - uint8_t ESC_STATUS; - int16_t speed; -#ifdef VOLTAGE_SENSOR_HAVE - uint16_t voltage; -#endif -#ifdef CURRENT_SENSOR_HAVE - uint16_t current; -#endif -#ifdef TEMPERATURE_SENSOR_HAVE - uint8_t temperature; -#endif -} ESC_FEEDBACK_DATA; +/****** Run ***********/ + +#define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff +#define RUN_RED_LED_ON_MASK (uint16_t)0x0800 +#define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000 +#define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000 +#define RUN_LED_ON_MASK (uint16_t)0x3800 +#define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000 +#define RUN_REVERSE_MASK (uint16_t)0x8000 + +typedef struct { + + uint16_t rpm_flags[TAP_ESC_MAX_MOTOR_NUM]; +} RunReq; + +typedef struct { + uint8_t channelID; + uint8_t ESCStatus; + int16_t speed; // -32767 - 32768 +#if defined(ESC_HAVE_VOLTAGE_SENSOR) + uint16_t voltage; // 0.00 - 100.00 V +#endif +#if defined(ESC_HAVE_CURRENT_SENSOR) + uint16_t current; // 0.0 - 200.0 A +#endif +#if defined(ESC_HAVE_TEMPERATURE_SENSOR) + uint8_t temperature; // 0 - 256 degree celsius +#endif +} RunInfoRepsonse; +/****** Run ***********/ + +/****** ConFigInfoBasic ***********/ +typedef struct { + uint8_t maxChannelInUse; + uint8_t channelMapTable[TAP_ESC_MAX_MOTOR_NUM]; + uint8_t monitorMsgType; + uint8_t controlMode; + uint16_t minChannelValue; + uint16_t maxChannelValue; +} ConfigInfoBasicRequest; + +typedef struct { + uint8_t channelID; + ConfigInfoBasicRequest resp; +} ConfigInfoBasicResponse; + +#define ESC_CHANNEL_MAP_CHANNEL 0x0f +#define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0 +/****** ConFigInfoBasicResponse ***********/ + +/****** InfoRequest ***********/ +typedef enum { + REQEST_INFO_BASIC = 0, + REQEST_INFO_FUll, + REQEST_INFO_RUN, + REQEST_INFO_STUDY, + REQEST_INFO_COMM, + REQEST_INFO_DEVICE, +} InfoTypes; + +typedef struct { + uint8_t channelID; + uint8_t requestInfoType; +} InfoRequest; + +/****** InfoRequest ***********/ typedef struct { uint8_t head; uint8_t len; uint8_t msg_id; - uint8_t data[100]; + union { + InfoRequest reqInfo; + ConfigInfoBasicRequest reqConfigInfoBasic; + RunReq reqRun; + + ConfigInfoBasicResponse rspConfigInfoBasic; + RunInfoRepsonse rspRunInfo; + uint8_t bytes[100]; + } d; uint8_t crc_data; -} ESC_FEEDBACK_PACKET; +} EscPacket; +#define UART_BUFFER_SIZE 128 typedef struct { uint8_t head; uint8_t tail; uint8_t dat_cnt; - uint8_t esc_feedback_buf[128]; + uint8_t esc_feedback_buf[UART_BUFFER_SIZE]; } ESC_UART_BUF; #pragma pack(pop) @@ -140,6 +212,8 @@ typedef enum { ESC_STATUS_ERROR_HARDWARE, ESC_STATUS_ERROR_LOSE_PROPELLER, ESC_STATUS_ERROR_OVER_CURRENT, + ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP, + ESC_STATUS_ERROR_LOSE_CMD, } ESCBUS_ENUM_ESC_STATUS; diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 65bb927790..cfc40430fa 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -60,15 +61,15 @@ #define B250000 250000 #endif -#define VOLTAGE_SENSOR_HAVE +#define ESC_HAVE_CURRENT_SENSOR #include "drv_tap_esc.h" /* * This driver connects to TAP ESCs via serial. */ -static const uint8_t crcTable[256] = TAP_ESC_CRC; -static int _uart_fd = -1; + +static int _uart_fd = -1; //todo:refactor in to class class TAP_ESC : public device::CDev { public: @@ -85,13 +86,18 @@ public: MODE_5CAP, MODE_6CAP, }; - TAP_ESC(); + TAP_ESC(int channels_count); virtual ~TAP_ESC(); virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); void cycle(); +protected: + void select_responder(uint8_t sel); private: + static const uint8_t crcTable[256]; + static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; + bool IS_armed; unsigned _poll_fds_num; @@ -104,6 +110,8 @@ private: actuator_outputs_s _outputs; static actuator_armed_s _armed; + //todo:refactor dynamic based on _num_outputs or _channels_count + // It needs to support the numbe of ESC int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; @@ -114,6 +122,7 @@ private: orb_advert_t _esc_feedback_pub = nullptr; esc_status_s _esc_feedback; + uint8_t _channels_count; // The numbe of ESC channels MixerGroup *_mixers; uint32_t _groups_required; @@ -122,22 +131,26 @@ private: unsigned _pwm_default_rate; unsigned _current_update_rate; ESC_UART_BUF uartbuf; - ESC_FEEDBACK_DATA feed_back_data; - ESC_FEEDBACK_PACKET feed_back_packet; + EscPacket _packet; void subscribe(); void work_start(); void work_stop(); void send_esc_outputs(const float *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, ESC_FEEDBACK_PACKET *packetdata); + 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); }; +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; + actuator_armed_s TAP_ESC::_armed = {}; namespace @@ -147,7 +160,7 @@ TAP_ESC *tap_esc; # define TAP_ESC_DEVICE_PATH "/dev/tap_esc" -TAP_ESC::TAP_ESC(): +TAP_ESC::TAP_ESC(int channels_count): CDev("tap_esc", TAP_ESC_DEVICE_PATH), IS_armed(false), _poll_fds_num(0), @@ -159,6 +172,7 @@ TAP_ESC::TAP_ESC(): _control_subs{ -1}, _esc_feedback_pub(nullptr), _esc_feedback{}, + _channels_count(channels_count), _mixers(nullptr), _groups_required(0), _groups_subscribed(0), @@ -208,14 +222,126 @@ TAP_ESC::init() ASSERT(!_initialized); - /* do regular cdev init */ - ret = CDev::init(); + /* Respect boot time requierd by the ESC FW */ - if (ret != OK) { + hrt_abstime uptime_us = hrt_absolute_time(); + + if (uptime_us < MAX_BOOT_TIME_MS * 1000) { + usleep((MAX_BOOT_TIME_MS * 1000) - uptime_us); + } + + /* Issue Basic Config */ + + EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC}; + ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic; + memset(&config, sizeof(ConfigInfoBasicRequest), 0); + config.maxChannelInUse = _channels_count; + + /* Asign the id's to the ESCs to match the mux */ + + for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) { + config.channelMapTable[phy_chan_index] = device_mux_map[phy_chan_index] & + ESC_CHANNEL_MAP_CHANNEL; // Use ESC_CHANNEL_MAP_RUNNING_DIRECTION; + } + + config.maxChannelValue = RPMMAX; + config.minChannelValue = RPMMIN; + + ret = send_packet(packet, 0); + + if (ret < 0) { return ret; } - return OK; + /* Verify All ESC got the config */ + + for (uint8_t cid = 0; cid < _channels_count; cid++) { + + /* Send the InfoRequest querying CONFIG_BASIC */ + EscPacket packet_info = {0xfe, sizeof(InfoRequest), ESCBUS_MSG_ID_REQUEST_INFO}; + InfoRequest &info_req = packet_info.d.reqInfo; + info_req.channelID = cid; + info_req.requestInfoType = REQEST_INFO_BASIC; + + ret = send_packet(packet_info, cid); + + if (ret < 0) { + return ret; + } + + /* Get a response */ + + int retries = 10; + bool valid = false; + + while (retries--) { + + read_data_from_uart(); + + if (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))); + break; + + } else { + + /* Give it time to come in */ + + usleep(1000); + } + } + + if (retries == 0 || !valid) { + return -EIO; + } + } + + /* To Unlock the ESC from the Power up state we need to issue 10 + * ESCBUS_MSG_ID_RUN request with all the values 0; + */ + + EscPacket unlock_packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; + unlock_packet.len *= sizeof(unlock_packet.d.reqRun.rpm_flags[0]); + memset(unlock_packet.d.bytes, 0, sizeof(packet.d.bytes)); + + int unlock_times = 10; + + while (unlock_times--) { + + send_packet(unlock_packet, -1); + + /* Min Packet to Packet time is 1 Ms so use 2 */ + + usleep(1000 * 2); + } + + /* do regular cdev init */ + + ret = CDev::init(); + + return ret; +} + +int TAP_ESC::send_packet(EscPacket &packet, int responder) +{ + if (responder >= 0) { + + if (responder > _channels_count) { + return -EINVAL; + } + + 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 @@ -257,12 +383,27 @@ uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len) 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::select_responder(uint8_t sel) +{ + px4_arch_gpiowrite(GPIO_S0, sel & 1); + px4_arch_gpiowrite(GPIO_S1, sel & 2); + px4_arch_gpiowrite(GPIO_S2, sel & 4); + +} + + void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm) { uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM]; + memset(rpm, 0, sizeof(rpm)); uint8_t motor_cnt = num_pwm; - unsigned char ESCData[TAP_ESC_MAX_PACKET_LEN] = {0}; static uint8_t which_to_respone = 0; for (uint8_t i = 0; i < motor_cnt; i++) { @@ -276,55 +417,22 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm) } } - switch (which_to_respone) { - case 0: rpm[0] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 1: rpm[1] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 2: rpm[2] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 3: rpm[3] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 4: rpm[4] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 5: rpm[5] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 6: rpm[6] |= RUN_FEEDBACK_ENABLE_MASK; - break; - - case 7: rpm[7] |= RUN_FEEDBACK_ENABLE_MASK; - - default: break; + rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK); + EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; + packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]); + + for (uint8_t i = 0; i < _channels_count; i++) { + packet.d.reqRun.rpm_flags[i] = rpm[i]; } - if (++which_to_respone == motor_cnt) { + int ret = send_packet(packet, which_to_respone); + + if (++which_to_respone == _channels_count) { which_to_respone = 0; } - //rpm[2]|=RUN_FEEDBACK_ENABLE_MASK; - - ESCData[0] = 0xFE; - ESCData[1] = motor_cnt * 2; - ESCData[2] = ESCBUS_MSG_ID_RUN; - - for (uint8_t i = 0; i < motor_cnt; i++) { - ESCData[3 + 2 * i] = rpm[i] & 0x00ff; - ESCData[3 + 2 * i + 1] = rpm[i] >> 8; - } - - ESCData[3 + 2 * motor_cnt] = crc8_esc(ESCData + 1, motor_cnt * 2 + 2); - - int ret = ::write(_uart_fd, &ESCData[0], motor_cnt * 2 + 4); - - if (ret < 1) { PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); } @@ -332,30 +440,32 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm) void TAP_ESC::read_data_from_uart() { - uint8_t tmp_serial_buf[128] = {0}; + uint8_t tmp_serial_buf[UART_BUFFER_SIZE] = {0}; - int len =::read(_uart_fd, &tmp_serial_buf[0], 128); + int len =::read(_uart_fd, &tmp_serial_buf[0], arraySize(tmp_serial_buf)); - if (len > 0 && (uartbuf.dat_cnt + len < 128)) { + 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 >= 128) { + if (uartbuf.tail >= UART_BUFFER_SIZE) { uartbuf.tail = 0; } } } } -bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PACKET *packetdata) +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) { - for (int i = 0; i < serial_buf->dat_cnt; i++) { + 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 @@ -364,7 +474,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC break; - case LEN: if (serial_buf->esc_feedback_buf[serial_buf->head] < 100) { + 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; @@ -385,7 +495,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC break; - case DATA: packetdata->data[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head]; + case DATA: packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head]; if (data_index >= packetdata->len) { @@ -415,7 +525,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC } - if (++serial_buf->head >= 128) { + if (++serial_buf->head >= UART_BUFFER_SIZE) { serial_buf->head = 0; } @@ -434,7 +544,6 @@ TAP_ESC::cycle() { if (!_initialized) { - _current_update_rate = 0; /* advertise the mixed control outputs, insist on the first group output */ _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); @@ -603,13 +712,13 @@ TAP_ESC::cycle() send_esc_outputs(motor_out, esc_count); read_data_from_uart(); - if (parse_tap_esc_feedback(&uartbuf, &feed_back_packet) == true) { - if (feed_back_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) { - feed_back_data = *(ESC_FEEDBACK_DATA *)feed_back_packet.data; - _esc_feedback.esc[feed_back_data.ESC_ID].esc_rpm = feed_back_data.speed; - _esc_feedback.esc[feed_back_data.ESC_ID].esc_voltage = feed_back_data.voltage; - _esc_feedback.esc[feed_back_data.ESC_ID].esc_state = feed_back_data.ESC_STATUS; - _esc_feedback.esc[feed_back_data.ESC_ID].esc_vendor = esc_status_s::ESC_VENDOR_TAP; + if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) { + if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) { + RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo; + _esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed; +// _esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage; + _esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus; + _esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP; // printf("vol is %d\n",feed_back_data.voltage ); // printf("speed is %d\n",feed_back_data.speed ); @@ -776,6 +885,7 @@ volatile bool _task_should_exit = false; // flag indicating if tap_esc task shou static char _device[32] = {}; static bool _is_running = false; // flag indicating if tap_esc app is running 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; @@ -802,7 +912,7 @@ int tap_esc_start(void) if (tap_esc == nullptr) { - tap_esc = new TAP_ESC; + tap_esc = new TAP_ESC(_supported_channel_count); if (tap_esc == nullptr) { ret = -ENOMEM; @@ -947,7 +1057,7 @@ void start() _task_handle = px4_task_spawn_cmd("tap_esc_main", SCHED_DEFAULT, SCHED_PRIORITY_MAX, - 1500, + 2500, (px4_main_t)&task_main_trampoline, nullptr); @@ -973,7 +1083,7 @@ void stop() void usage() { - PX4_INFO("usage: tap_esc start -d /dev/ttyS2"); + PX4_INFO("usage: tap_esc start -d /dev/ttyS2 -n <1-8>"); PX4_INFO(" tap_esc stop"); PX4_INFO(" tap_esc status"); } @@ -996,12 +1106,16 @@ int tap_esc_main(int argc, char *argv[]) verb = argv[1]; } - while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': device = myoptarg; strncpy(tap_esc_drv::_device, device, strlen(device)); break; + + case 'n': + tap_esc_drv::_supported_channel_count = atoi(myoptarg); + break; } } @@ -1013,7 +1127,7 @@ int tap_esc_main(int argc, char *argv[]) } // Check on required arguments - if (device == nullptr || strlen(device) == 0) { + if (tap_esc_drv::_supported_channel_count == 0 || device == nullptr || strlen(device) == 0) { tap_esc_drv::usage(); return 1; }