diff --git a/src/drivers/boards/aerofc-v1/board_config.h b/src/drivers/boards/aerofc-v1/board_config.h index 23179e66d2a9..5c6ddb6298cf 100644 --- a/src/drivers/boards/aerofc-v1/board_config.h +++ b/src/drivers/boards/aerofc-v1/board_config.h @@ -142,8 +142,6 @@ /* * ESCs do not respond */ -#define BOARD_TAP_ESC_NO_VERIFY_CONFIG 1 - #define BOARD_TAP_ESC_MODE 1 #define MEMORY_CONSTRAINED_SYSTEM diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h index c7dadc521227..37aa5cdaf58c 100644 --- a/src/drivers/tap_esc/drv_tap_esc.h +++ b/src/drivers/tap_esc/drv_tap_esc.h @@ -35,12 +35,16 @@ #include +#include + /* At the moment the only known use is with a current sensor */ #define ESC_HAVE_CURRENT_SENSOR #define TAP_ESC_MAX_PACKET_LEN 20 #define TAP_ESC_MAX_MOTOR_NUM 8 +#define PACKET_HEAD 0xfe + /* 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. @@ -117,18 +121,18 @@ typedef struct { ConfigInfoBasicRequest resp; } ConfigInfoBasicResponse; -#define ESC_CHANNEL_MAP_CHANNEL 0x0f -#define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0 +#define ESC_MASK_MAP_CHANNEL 0x0f +#define ESC_MASK_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, + REQUEST_INFO_BASIC = 0, + REQUEST_INFO_FUll, + REQUEST_INFO_RUN, + REQUEST_INFO_STUDY, + REQUEST_INFO_COMM, + REQUEST_INFO_DEVICE, } InfoTypes; typedef struct { diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 15ea9385a515..b3ad6b2acf30 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,15 +33,16 @@ #include +#include +#include #include #include #include #include -#include + #include // NAN #include -#include #include #include #include @@ -61,151 +62,109 @@ #include "tap_esc_common.h" -#ifndef B250000 -#define B250000 250000 -#endif - #include "drv_tap_esc.h" -#if !defined(BOARD_TAP_ESC_NO_VERIFY_CONFIG) -# define BOARD_TAP_ESC_NO_VERIFY_CONFIG 0 -#endif - #if !defined(BOARD_TAP_ESC_MODE) # define BOARD_TAP_ESC_MODE 0 #endif +#if !defined(DEVICE_ARGUMENT_MAX_LENGTH) +# define DEVICE_ARGUMENT_MAX_LENGTH 32 +#endif + +#if !defined(PWM_DEFAULT_UPDATE_RATE) +# define PWM_DEFAULT_UPDATE_RATE 400 +#endif + +#define TAP_ESC_DEVICE_PATH "/dev/tap_esc" + /* * This driver connects to TAP ESCs via serial. */ - -static int _uart_fd = -1; //todo:refactor in to class -class TAP_ESC : public device::CDev, public ModuleParams +class TAP_ESC : public device::CDev, public ModuleBase, public ModuleParams { public: - enum Mode { - MODE_NONE, - MODE_2PWM, - MODE_2PWM2CAP, - MODE_3PWM, - MODE_3PWM1CAP, - MODE_4PWM, - MODE_6PWM, - MODE_8PWM, - MODE_4CAP, - MODE_5CAP, - MODE_6CAP, - }; - TAP_ESC(int channels_count); + TAP_ESC(char const *const device, uint8_t channels_count); virtual ~TAP_ESC(); - virtual int init(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - void cycle(); - -private: - static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; - static const uint8_t device_dir_map[TAP_ESC_MAX_MOTOR_NUM]; + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - bool _is_armed; + /** @see ModuleBase */ + static TAP_ESC *instantiate(int argc, char *argv[]); - unsigned _poll_fds_num; - Mode _mode; - // subscriptions - int _armed_sub; - int _test_motor_sub; - int _params_sub; - orb_advert_t _outputs_pub; - actuator_outputs_s _outputs; - actuator_armed_s _armed; + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - //todo:refactor dynamic based on _channels_count - // It needs to support the number of ESC - int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - - actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + /** @see ModuleBase::run() */ + void run() override; - orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + virtual int init(); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + void cycle(); - orb_advert_t _esc_feedback_pub = nullptr; - orb_advert_t _to_mixer_status; ///< mixer status flags - esc_status_s _esc_feedback; - uint8_t _channels_count; // The number of ESC channels +private: + char _device[DEVICE_ARGUMENT_MAX_LENGTH]; + int _uart_fd = -1; + static const uint8_t _device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; + static const uint8_t _device_dir_map[TAP_ESC_MAX_MOTOR_NUM]; + bool _is_armed = false; + int _armed_sub = -1; + int _test_motor_sub = -1; + int _params_sub = -1; + orb_advert_t _outputs_pub = nullptr; + actuator_outputs_s _outputs = {}; + actuator_armed_s _armed = {}; + + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num = 0; - MixerGroup *_mixers; - uint32_t _groups_required; - uint32_t _groups_subscribed; - volatile bool _initialized; - unsigned _pwm_default_rate; - unsigned _current_update_rate; - ESC_UART_BUF uartbuf; - EscPacket _packet; + orb_advert_t _esc_feedback_pub = nullptr; + orb_advert_t _to_mixer_status = nullptr; ///< mixer status flags + esc_status_s _esc_feedback = {}; + uint8_t _channels_count = 0; ///< nnumber of ESC channels + uint8_t _responding_esc = 0; + MixerGroup *_mixers = nullptr; + uint32_t _groups_required = 0; + uint32_t _groups_subscribed = 0; + ESC_UART_BUF _uartbuf = {}; + EscPacket _packet = {}; DEFINE_PARAMETERS( (ParamBool) _airmode ///< multicopter air-mode ) - void subscribe(); - - 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); + void subscribe(); + void send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt); 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); }; -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_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS; +const uint8_t TAP_ESC::_device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR; -namespace -{ -TAP_ESC *tap_esc = nullptr; -} - -# define TAP_ESC_DEVICE_PATH "/dev/tap_esc" - -TAP_ESC::TAP_ESC(int channels_count): +TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count): CDev("tap_esc", TAP_ESC_DEVICE_PATH), ModuleParams(nullptr), - _is_armed(false), - _poll_fds_num(0), - _mode(MODE_4PWM), //FIXME: what is this mode used for??? - _armed_sub(-1), - _test_motor_sub(-1), - _params_sub(-1), - _outputs_pub(nullptr), - _armed{}, - _esc_feedback_pub(nullptr), - _to_mixer_status(nullptr), - _esc_feedback{}, - _channels_count(channels_count), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _initialized(false), - _pwm_default_rate(400), - _current_update_rate(0) - + _channels_count(channels_count) { + strncpy(_device, device, sizeof(_device)); + _device[sizeof(_device) - 1] = '\0'; // Fix in case of overflow + _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); - uartbuf.head = 0; - uartbuf.tail = 0; - uartbuf.dat_cnt = 0; - 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; @@ -220,35 +179,99 @@ TAP_ESC::TAP_ESC(int channels_count): TAP_ESC::~TAP_ESC() { - if (_initialized) { - /* tell the task we want it to go away */ - work_stop(); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + orb_unsubscribe(_control_subs[i]); + _control_subs[i] = -1; + } + } + + orb_unsubscribe(_armed_sub); + orb_unsubscribe(_test_motor_sub); + orb_unsubscribe(_params_sub); + + orb_unadvertise(_outputs_pub); + orb_unadvertise(_esc_feedback_pub); + orb_unadvertise(_to_mixer_status); + + tap_esc_common::deinitialise_uart(_uart_fd); + + DEVICE_LOG("stopping"); +} + +/** @see ModuleBase */ +TAP_ESC *TAP_ESC::instantiate(int argc, char *argv[]) +{ + /* Parse arguments */ + const char *device = nullptr; + uint8_t channels_count = 0; + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + if (argc < 2) { + print_usage("not enough arguments"); + return nullptr; + } + + while ((ch = px4_getopt(argc, argv, "d:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + break; - int i = 10; + case 'n': + channels_count = atoi(myoptarg); + break; + } + } - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - i--; + /* Sanity check on arguments */ + if (channels_count == 0) { + print_usage("Channel count is invalid (0)"); + return nullptr; + } - } while (_initialized && i > 0); + if (device == nullptr || strlen(device) == 0) { + print_usage("no device specified"); + return nullptr; } - // clean up the alternate device node - //unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); + TAP_ESC *tap_esc = new TAP_ESC(device, channels_count); - tap_esc = nullptr; + if (tap_esc == nullptr) { + PX4_ERR("failed to instantiate module"); + return nullptr; + } + + if (tap_esc->init() != 0) { + PX4_ERR("failed to initialize module"); + delete tap_esc; + return nullptr; + } + + return tap_esc; +} + +/** @see ModuleBase */ +int TAP_ESC::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); } -int -TAP_ESC::init() +int TAP_ESC::init() { int ret; - ASSERT(!_initialized); + ret = tap_esc_common::initialise_uart(_device, _uart_fd); - /* Respect boot time required by the ESC FW */ + if (ret != 0) { + PX4_ERR("failed to initialise UART."); + return ret; + } + /* Respect boot time required by the ESC FW */ hrt_abstime uptime_us = hrt_absolute_time(); if (uptime_us < MAX_BOOT_TIME_MS * 1000) { @@ -256,8 +279,7 @@ TAP_ESC::init() } /* Issue Basic Config */ - - EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC}; + EscPacket packet = {PACKET_HEAD, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC}; ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic; memset(&config, 0, sizeof(ConfigInfoBasicRequest)); config.maxChannelInUse = _channels_count; @@ -265,121 +287,56 @@ TAP_ESC::init() config.controlMode = BOARD_TAP_ESC_MODE; /* 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; - config.channelMapTable[phy_chan_index] |= (device_dir_map[phy_chan_index] << 4) & - ESC_CHANNEL_MAP_RUNNING_DIRECTION; + config.channelMapTable[phy_chan_index] = _device_mux_map[phy_chan_index] & + ESC_MASK_MAP_CHANNEL; + config.channelMapTable[phy_chan_index] |= (_device_dir_map[phy_chan_index] << 4) & + ESC_MASK_MAP_RUNNING_DIRECTION; } 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; } -#if !BOARD_TAP_ESC_NO_VERIFY_CONFIG - - /* 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 (!valid) { - return -EIO; - } - - } - -#endif - /* 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}; + EscPacket unlock_packet = {PACKET_HEAD, _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)); + memset(unlock_packet.d.bytes, 0, sizeof(unlock_packet.d.bytes)); int unlock_times = 10; 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 */ - - usleep(1000 * 2); + usleep(2000); } /* do regular cdev init */ - ret = CDev::init(); - return ret; -} + /* advertise the mixed control outputs, insist on the first group output */ + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + _esc_feedback_pub = orb_advertise(ORB_ID(esc_status), &_esc_feedback); + multirotor_motor_limits_s multirotor_motor_limits = {}; + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits); -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); - } + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); return ret; } -void -TAP_ESC::subscribe() +void TAP_ESC::subscribe() { /* subscribe/unsubscribe to required actuator control groups */ uint32_t sub_groups = _groups_required & ~_groups_subscribed; @@ -406,31 +363,10 @@ TAP_ESC::subscribe() } } -uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len) +void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt) { - 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) -{ - uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM]; memset(rpm, 0, sizeof(rpm)); - uint8_t motor_cnt = num_pwm; - static uint8_t which_to_respone = 0; for (uint8_t i = 0; i < motor_cnt; i++) { rpm[i] = pwm[i]; @@ -443,20 +379,19 @@ void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm) } } - rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK); + rpm[_responding_esc] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK); - - EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN}; + EscPacket packet = {PACKET_HEAD, _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]; } - int ret = send_packet(packet, which_to_respone); + int ret = tap_esc_common::send_packet(_uart_fd, packet, _responding_esc); - if (++which_to_respone == _channels_count) { - which_to_respone = 0; + if (++_responding_esc == _channels_count) { + _responding_esc = 0; } if (ret < 1) { @@ -464,149 +399,21 @@ 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) +void TAP_ESC::cycle() { - 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() -{ - - 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); - _esc_feedback_pub = orb_advertise(ORB_ID(esc_status), &_esc_feedback); - multirotor_motor_limits_s multirotor_motor_limits = {}; - _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _initialized = true; - } - if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - _current_update_rate = 0; - } - unsigned max_rate = _pwm_default_rate ; + /* Set uorb update rate */ + int update_rate_in_ms = int(1000 / PWM_DEFAULT_UPDATE_RATE); - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { + /* reject faster than 500 Hz updates */ update_rate_in_ms = 2; - } - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { + } else if (update_rate_in_ms > 100) { + /* reject slower than 10 Hz updates */ update_rate_in_ms = 100; } @@ -617,20 +424,15 @@ TAP_ESC::cycle() orb_set_interval(_control_subs[i], update_rate_in_ms); } } - - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; } if (_mixers) { _mixers->set_airmode(_airmode.get()); } - /* check if anything updated */ int ret = px4_poll(_poll_fds, _poll_fds_num, 5); - /* this would be bad... */ if (ret < 0) { DEVICE_LOG("poll error %d", errno); @@ -651,7 +453,7 @@ TAP_ESC::cycle() } } - size_t num_outputs = _channels_count; + uint8_t num_outputs = _channels_count; /* can we mix? */ if (_is_armed && _mixers != nullptr) { @@ -722,52 +524,56 @@ TAP_ESC::cycle() } - const unsigned esc_count = num_outputs; uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM]; // We need to remap from the system default to what PX4's normal // scheme is - if (num_outputs == 6) { + switch (num_outputs) { + case 4: + motor_out[0] = (uint16_t)_outputs.output[2]; + motor_out[1] = (uint16_t)_outputs.output[1]; + motor_out[2] = (uint16_t)_outputs.output[0]; + motor_out[3] = (uint16_t)_outputs.output[3]; + break; + + case 6: motor_out[0] = (uint16_t)_outputs.output[3]; motor_out[1] = (uint16_t)_outputs.output[0]; motor_out[2] = (uint16_t)_outputs.output[4]; motor_out[3] = (uint16_t)_outputs.output[2]; motor_out[4] = (uint16_t)_outputs.output[1]; motor_out[5] = (uint16_t)_outputs.output[5]; - motor_out[6] = RPMSTOPPED; - motor_out[7] = RPMSTOPPED; + break; - } else if (num_outputs == 4) { - motor_out[0] = (uint16_t)_outputs.output[2]; - motor_out[2] = (uint16_t)_outputs.output[0]; - motor_out[1] = (uint16_t)_outputs.output[1]; - motor_out[3] = (uint16_t)_outputs.output[3]; + default: - } else { // Use the system defaults - for (unsigned i = 0; i < esc_count; ++i) { + for (uint8_t i = 0; i < num_outputs; ++i) { motor_out[i] = (uint16_t)_outputs.output[i]; } + + break; } - send_esc_outputs(motor_out, esc_count); - read_data_from_uart(); + // Set remaining motors to RPMSTOPPED to be on the safe side + for (uint8_t i = num_outputs; i < TAP_ESC_MAX_MOTOR_NUM; ++i) { + motor_out[i] = RPMSTOPPED; + } + + send_esc_outputs(motor_out, num_outputs); + 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)) { if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) { RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo; if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) { _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 ); - _esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; _esc_feedback.counter++; - _esc_feedback.esc_count = esc_count; + _esc_feedback.esc_count = num_outputs; _esc_feedback.timestamp = hrt_absolute_time(); @@ -808,27 +614,6 @@ TAP_ESC::cycle() orb_copy(ORB_ID(parameter_update), _params_sub, &update); updateParams(); } - - -} - -void TAP_ESC::work_stop() -{ - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } - } - - orb_unsubscribe(_armed_sub); - _armed_sub = -1; - orb_unsubscribe(_test_motor_sub); - _test_motor_sub = -1; - orb_unsubscribe(_params_sub); - - DEVICE_LOG("stopping"); - _initialized = false; } int TAP_ESC::control_callback_trampoline(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) @@ -849,18 +634,6 @@ int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, floa input = -1.0f; } - /* motor spinup phase - lock throttle to zero */ - // if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { - // if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - // control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - // control_index == actuator_controls_s::INDEX_THROTTLE) { - // /* limit the throttle output to zero during motor spinup, - // * as the motors cannot follow any demand yet - // */ - // input = 0.0f; - // } - // } - /* throttle not arming - mark throttle input as invalid */ if (_armed.prearmed && !_armed.armed) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || @@ -874,8 +647,7 @@ int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, floa return 0; } -int -TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) +int TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = OK; @@ -927,294 +699,78 @@ TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - - return ret; } -namespace tap_esc_drv -{ - - -volatile bool _task_should_exit = false; // flag indicating if tap_esc task should exit -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; - -void usage(); - -void start(); -void stop(); -int tap_esc_start(void); -int tap_esc_stop(void); - -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) +/** @see ModuleBase */ +void TAP_ESC::run() { - int ret = OK; - - if (tap_esc == nullptr) { - - tap_esc = new TAP_ESC(_supported_channel_count); - - if (tap_esc == nullptr) { - ret = -ENOMEM; - - } else { - ret = tap_esc->init(); - - if (ret != OK) { - PX4_ERR("failed to initialize tap_esc (%i)", ret); - delete tap_esc; - tap_esc = nullptr; - } - } - } - - return ret; -} - -int tap_esc_stop(void) -{ - int ret = OK; - - if (tap_esc != nullptr) { - - delete tap_esc; - tap_esc = nullptr; + // Main loop + while (!should_exit()) { + cycle(); } - - return ret; } -int initialise_uart() +/** @see ModuleBase */ +int TAP_ESC::task_spawn(int argc, char *argv[]) { - // 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; + /* start the task */ + _task_id = px4_task_spawn_cmd("tap_esc", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 1100, + (px4_main_t)&run_trampoline, + argv); + + if (_task_id < 0) { + PX4_ERR("task start failed"); + _task_id = -1; + return PX4_ERROR; } - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - PX4_ERR("tcsetattr failed for %s\n", _device); - close(_uart_fd); + // wait until task is up & running + if (wait_until_running() < 0) { + _task_id = -1; return -1; } - // setup output flow control - if (enable_flow_control(false)) { - PX4_WARN("hardware flow disable failed"); - } - - return _uart_fd; + return PX4_OK; } -int enable_flow_control(bool enabled) +/** @see ModuleBase */ +int TAP_ESC::print_usage(const char *reason) { - 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; + if (reason) { + PX4_WARN("%s\n", reason); } - return ret; -} - -int deinitialize_uart() -{ - return close(_uart_fd); -} + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module controls the TAP_ESC hardware via UART. It listens on the +actuator_controls topics, does the mixing and writes the PWM outputs. -void task_main(int argc, char *argv[]) -{ +### Implementation +Currently the module is implementd as a threaded version only, meaning that it +runs in its own thread instead of on the work queue. - _is_running = true; +### Example +The module is typically started with: +tap_esc start -d /dev/ttyS2 -n <1-8> - if (initialise_uart() < 0) { - PX4_ERR("Failed to initialize UART."); +)DESCR_STR"); - while (_task_should_exit == false) { - usleep(100000); - } - - _is_running = false; - return; - } - - if (tap_esc_start() != OK) { - PX4_ERR("failed to start tap_esc."); - _is_running = false; - return; - } + PRINT_MODULE_USAGE_NAME("tap_esc", "driver"); - - // Main loop - while (!_task_should_exit) { - - tap_esc->cycle(); - - } - - - _is_running = false; + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Device used to talk to ESCs", true); + PRINT_MODULE_USAGE_PARAM_INT('n', 4, 0, 8, "Number of ESCs", true); + return PX4_OK; } -void task_main_trampoline(int argc, char *argv[]) -{ - task_main(argc, argv); -} - -void start() -{ - ASSERT(_task_handle == -1); - - _task_should_exit = false; - - /* start the task */ - _task_handle = px4_task_spawn_cmd("tap_esc", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1100, - (px4_main_t)&task_main_trampoline, - nullptr); - - if (_task_handle < 0) { - PX4_ERR("task start failed"); - _task_handle = -1; - return; - } -} - -void stop() -{ - _task_should_exit = true; - - while (_is_running) { - usleep(200000); - PX4_INFO("tap_esc_stop"); - } - - tap_esc_stop(); - deinitialize_uart(); - _task_handle = -1; -} - -void usage() -{ - PX4_INFO("usage: tap_esc start -d /dev/ttyS2 -n <1-8>"); - PX4_INFO(" tap_esc stop"); - PX4_INFO(" tap_esc status"); -} - -} // namespace tap_esc - -// driver 'main' command extern "C" __EXPORT int tap_esc_main(int argc, char *argv[]); int tap_esc_main(int argc, char *argv[]) { - const char *device = nullptr; - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - char *verb = nullptr; - - if (argc >= 2) { - verb = argv[1]; - } - - 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; - } - } - - if (!tap_esc && tap_esc_drv::_task_handle != -1) { - tap_esc_drv::_task_handle = -1; - } - - // Start/load the driver. - if (!strcmp(verb, "start")) { - if (tap_esc_drv::_is_running) { - PX4_WARN("tap_esc already running"); - return 1; - } - - // Check on required arguments - if (tap_esc_drv::_supported_channel_count == 0 || device == nullptr || strlen(device) == 0) { - tap_esc_drv::usage(); - return 1; - } - - tap_esc_drv::start(); - } - - else if (!strcmp(verb, "stop")) { - if (!tap_esc_drv::_is_running) { - PX4_WARN("tap_esc is not running"); - return 1; - } - - tap_esc_drv::stop(); - } - - else if (!strcmp(verb, "status")) { - PX4_WARN("tap_esc is %s", tap_esc_drv::_is_running ? "running" : "not running"); - return 0; - - } else { - tap_esc_drv::usage(); - return 1; - } - - return 0; + return TAP_ESC::main(argc, argv); } diff --git a/src/drivers/tap_esc/tap_esc_common.cpp b/src/drivers/tap_esc/tap_esc_common.cpp index 962fa9939cde..6961f22686dc 100644 --- a/src/drivers/tap_esc/tap_esc_common.cpp +++ b/src/drivers/tap_esc/tap_esc_common.cpp @@ -36,20 +36,24 @@ * */ -#include #include "tap_esc_common.h" +#include +#include + +#include // arraySize +#include +#include + +#ifndef B250000 +#define B250000 250000 +#endif namespace tap_esc_common { -/**************************************************************************** - * Name: select_responder - * - * Description: - * Select tap esc responder for serial interface (device 74hct151). - * GPIOs to be defined in board_config.h - * - ****************************************************************************/ +static uint8_t crc8_esc(uint8_t *p, uint8_t len); +static uint8_t crc_packet(EscPacket &p); + void select_responder(uint8_t sel) { #if defined(GPIO_S0) @@ -59,6 +63,222 @@ 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 0; +} + +int deinitialise_uart(int &uart_fd) +{ + int ret = close(uart_fd); + + if (ret == 0) { + uart_fd = -1; + } + + return ret; +} + +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; +} + +int 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; + } + } + + } else if (len < 0) { + return len; + } + + return 0; +} + +int 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] == PACKET_HEAD) { + packetdata->head = PACKET_HEAD; //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 0; + } + + state = HEAD; + break; + + default: + state = HEAD; + break; + + } + + if (++serial_buf->head >= UART_BUFFER_SIZE) { + serial_buf->head = 0; + } + + serial_buf->dat_cnt--; + } + } + + return -1; +} + +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 bfa9e5100c90..59ee6111ba82 100644 --- a/src/drivers/tap_esc/tap_esc_common.h +++ b/src/drivers/tap_esc/tap_esc_common.h @@ -40,16 +40,67 @@ #include +#include "drv_tap_esc.h" + namespace tap_esc_common { -/**************************************************************************** - * Name: select_responder - * - * Description: - * Select tap esc responder data for serial interface by 74hct151 - * - ****************************************************************************/ +/** + * Select tap esc responder data for serial interface by 74hct151. GPIOs to be + * defined in board_config.h + * @param sel ID of the ESC (responder) of which feedback is requested + */ void select_responder(uint8_t sel); +/** + * Opens a device for use as UART. + * @param device UNIX path of UART device + * @param uart_fd file-descriptor of UART device + * @return 0 on success, -1 on error + */ +int initialise_uart(const char *const device, int &uart_fd); + +/** + * Closes a device previously opened with initialise_uart(). + * @param uart_fd file-descriptor of UART device as provided by initialise_uart() + * @return 0 on success, -1 on error + */ +int deinitialise_uart(int &uart_fd); + +/** + * Enables/disables flow control for open UART device. + * @param uart_fd file-descriptor of UART device + * @param enabled Set true for enabling and false for disabling flow control + * @return 0 on success, -1 on error + */ +int enable_flow_control(int uart_fd, bool enabled); + +/** + * Sends a packet to all ESCs and requests a specific ESC to respond + * @param uart_fd file-descriptor of UART device + * @param packet Packet to be sent to ESCs. CRC information will be added. + * @param responder ID of the ESC (responder) that should return feedback + * @return On success number of bytes written, on error -1 + */ +int send_packet(int uart_fd, EscPacket &packet, int responder); + +/** + * Read data from the UART into a buffer + * @param uart_fd file-descriptor of UART device + * @param uart_buf Buffer where incoming data will be stored + * @return 0 on success, -1 on error + */ +int read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf); + +/** + * Parse feedback from an ESC + * @param serial_buf Buffer where incoming data will be stored + * @param packetdata Packet that will be populated with information from buffer + * @return 0 on success, -1 on error + */ +int parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata); + +/** + * Lookup-table for faster CRC computation when sending ESC packets. + */ extern const uint8_t crc_table[]; } /* tap_esc_common */