diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h index 1c13146afc..883a60056b 100644 --- a/boards/atl/mantis-edu/src/board_config.h +++ b/boards/atl/mantis-edu/src/board_config.h @@ -115,6 +115,9 @@ #define HW_INFO_INIT_VER 2 #define HW_INFO_INIT_REV 3 +#define BOARD_TAP_ESC_MODE 2 // select closed-loop control mode for the esc +// #define BOARD_USE_ESC_CURRENT_REPORT + /* HEATER */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt new file mode 100644 index 0000000000..a9f711ad22 --- /dev/null +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2018-2021 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__tap_esc + MAIN tap_esc + COMPILE_FLAGS + SRCS + tap_esc_common.cpp + tap_esc_common.h + tap_esc_uploader.cpp + tap_esc_uploader.h + TAP_ESC.cpp + TAP_ESC.hpp + DEPENDS + led + mixer + tunes + ) + diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp new file mode 100644 index 0000000000..950cf831dc --- /dev/null +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -0,0 +1,779 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TAP_ESC.hpp" + +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; + +TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count): + CDev(TAP_ESC_DEVICE_PATH), + ModuleParams(nullptr), + _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); + + for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) { + _control_subs[i] = -1; + } + + for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + + _outputs.noutputs = 0; +} + +TAP_ESC::~TAP_ESC() +{ + 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; + } + } + + tap_esc_common::deinitialise_uart(_uart_fd); + + PX4_INFO("stopping"); + + perf_free(_perf_control_latency); +} + +/** @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; + + case 'n': + channels_count = atoi(myoptarg); + break; + } + } + + /* Sanity check on arguments */ + if (channels_count == 0) { + print_usage("Channel count is invalid (0)"); + return nullptr; + } + + if (device == nullptr || strlen(device) == 0) { + print_usage("no device specified"); + return nullptr; + } + + TAP_ESC *tap_esc = new TAP_ESC(device, channels_count); + + 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 ret = tap_esc_common::initialise_uart(_device, _uart_fd); + + 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) { + usleep((MAX_BOOT_TIME_MS * 1000) - uptime_us); + } + + /* Issue Basic Config */ + 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; + /* Enable closed-loop control if supported by the board */ + 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_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 = tap_esc_common::send_packet(_uart_fd, packet, 0); + + if (ret < 0) { + return ret; + } + + /* set wait time for tap esc configurate and write flash (0.02696s measure by Saleae logic Analyzer) */ + usleep(30000); + + /* Verify All ESC got the config */ + for (uint8_t cid = 0; cid < _channels_count; cid++) { + + /* Send the InfoRequest querying CONFIG_BASIC */ + EscPacket packet_info = {PACKET_HEAD, sizeof(InfoRequest), ESCBUS_MSG_ID_REQUEST_INFO}; + InfoRequest &info_req = packet_info.d.reqInfo; + info_req.channelID = _device_mux_map[cid];; + info_req.requestInfoType = REQUEST_INFO_BASIC; + + ret = tap_esc_common::send_packet(_uart_fd, packet_info, cid); + + if (ret < 0) { + return ret; + } + + /* Get a response */ + int retries = 10; + bool valid = false; + + while (retries--) { + tap_esc_common::read_data_from_uart(_uart_fd, &_uartbuf); + + if (tap_esc_common::parse_tap_esc_feedback(&_uartbuf, &_packet) == 0) { + valid = (_packet.msg_id == ESCBUS_MSG_ID_CONFIG_INFO_BASIC + && _packet.d.rspConfigInfoBasic.channelID == _device_mux_map[cid]); + break; + + } else { + /* Give it time to come in */ + usleep(1000); + } + } + + if (!valid) { + PX4_ERR("Verification of the configuration failed, ESC number: %d", cid); + 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{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(unlock_packet.d.bytes)); + + int unlock_times = 10; + + while (unlock_times--) { + tap_esc_common::send_packet(_uart_fd, unlock_packet, -1); + + /* Min Packet to Packet time is 1 Ms so use 2 */ + usleep(2000); + } + + /* do regular cdev init */ + ret = CDev::init(); + + return ret; +} + +void TAP_ESC::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); + orb_unsubscribe(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] >= 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt) +{ + uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM] {}; + _led_controller.update(_led_control_data); + + for (uint8_t i = 0; i < motor_cnt; i++) { + rpm[i] = pwm[i]; + + if ((rpm[i] & RUN_CHANNEL_VALUE_MASK) > RPMMAX) { + rpm[i] = (rpm[i] & ~RUN_CHANNEL_VALUE_MASK) | RPMMAX; + + } else if ((rpm[i] & RUN_CHANNEL_VALUE_MASK) < RPMSTOPPED) { + rpm[i] = (rpm[i] & ~RUN_CHANNEL_VALUE_MASK) | RPMSTOPPED; + } + + // apply the led color + if (i < BOARD_MAX_LEDS) { + switch (_led_control_data.leds[i].color) { + case led_control_s::COLOR_RED: + rpm[i] |= RUN_RED_LED_ON_MASK; + break; + + case led_control_s::COLOR_GREEN: + rpm[i] |= RUN_GREEN_LED_ON_MASK; + break; + + case led_control_s::COLOR_BLUE: + rpm[i] |= RUN_BLUE_LED_ON_MASK; + break; + + case led_control_s::COLOR_AMBER: //make it the same as yellow + case led_control_s::COLOR_YELLOW: + rpm[i] |= RUN_RED_LED_ON_MASK | RUN_GREEN_LED_ON_MASK; + break; + + case led_control_s::COLOR_PURPLE: + rpm[i] |= RUN_RED_LED_ON_MASK | RUN_BLUE_LED_ON_MASK; + break; + + case led_control_s::COLOR_CYAN: + rpm[i] |= RUN_GREEN_LED_ON_MASK | RUN_BLUE_LED_ON_MASK; + break; + + case led_control_s::COLOR_WHITE: + rpm[i] |= RUN_RED_LED_ON_MASK | RUN_GREEN_LED_ON_MASK | RUN_BLUE_LED_ON_MASK; + break; + + default: // led_control_s::COLOR_OFF + break; + } + } + } + + rpm[_responding_esc] |= RUN_FEEDBACK_ENABLE_MASK; + + 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 = tap_esc_common::send_packet(_uart_fd, packet, _responding_esc); + + if (++_responding_esc >= _channels_count) { + _responding_esc = 0; + } + + if (ret < 1) { + PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); + } +} + +void TAP_ESC::send_tune_packet(EscbusTunePacket &tune_packet) +{ + PX4_DEBUG("send_tune_packet: Frequency: %d Hz Duration %d ms, strength: %d", tune_packet.frequency, + tune_packet.duration_ms, tune_packet.strength); + + EscPacket buzzer_packet{PACKET_HEAD, sizeof(EscbusTunePacket), ESCBUS_MSG_ID_TUNE}; + buzzer_packet.d.tunePacket = tune_packet; + tap_esc_common::send_packet(_uart_fd, buzzer_packet, -1); +} + +void TAP_ESC::cycle() +{ + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + + /* Set uorb update rate */ + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + orb_set_interval(_control_subs[i], TAP_ESC_CTRL_UORB_UPDATE_INTERVAL); + PX4_DEBUG("New actuator update interval: %ums", TAP_ESC_CTRL_UORB_UPDATE_INTERVAL); + } + } + } + + if (_mixers) { + _mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get()); + } + + /* check if anything updated */ + int ret = px4_poll(_poll_fds, _poll_fds_num, 5); + + /* this would be bad... */ + if (ret < 0) { + PX4_ERR("poll error %d", errno); + + } else { /* update even in the case of a timeout, to check for test_motor commands */ + + /* get controls for required topics */ + unsigned poll_id = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + + } + + poll_id++; + } + } + + uint8_t num_outputs = _channels_count; + + /* can we mix? */ + if (_is_armed && _mixers != nullptr) { + + /* do mixing */ + num_outputs = _mixers->mix(&_outputs.output[0], num_outputs); + _outputs.noutputs = num_outputs; + + /* publish mixer status */ + multirotor_motor_limits_s multirotor_motor_limits{}; + multirotor_motor_limits.saturation_status = _mixers->get_saturation_status(); + multirotor_motor_limits.timestamp = hrt_absolute_time(); + _to_mixer_status.publish(multirotor_motor_limits); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < _outputs.noutputs && PX4_ISFINITE(_outputs.output[i]) + && !_armed.lockdown && !_armed.manual_lockdown) { + /* scale for PWM output 1200 - 1900us */ + _outputs.output[i] = (RPMMAX + RPMMIN) / 2 + ((RPMMAX - RPMMIN) / 2) * _outputs.output[i]; + math::constrain(_outputs.output[i], (float)RPMMIN, (float)RPMMAX); + + } else { + /* + * Value is NaN, INF, or we are in lockdown - stop the motor. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = RPMSTOPPED; + } + } + + } else { + _outputs.noutputs = num_outputs; + _outputs.timestamp = hrt_absolute_time(); + + /* check for motor test commands */ + if (_test_motor_sub.updated()) { + test_motor_s test_motor; + + if (_test_motor_sub.copy(&test_motor)) { + if (test_motor.action == test_motor_s::ACTION_STOP) { + _outputs.output[test_motor.motor_number] = RPMSTOPPED; + + } else { + _outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value); + } + + PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number, + (double)_outputs.output[test_motor.motor_number]); + } + } + + /* set the invalid values to the minimum */ + for (unsigned i = 0; i < num_outputs; i++) { + if (!PX4_ISFINITE(_outputs.output[i])) { + _outputs.output[i] = RPMSTOPPED; + } + } + + /* disable unused ports by setting their output to NaN */ + for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + } + + uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM] {}; + + // We need to remap from the system default to what PX4's normal + // scheme is + 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[3]; + motor_out[3] = (uint16_t)_outputs.output[0]; + 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]; + break; + + default: + + // Use the system defaults + for (uint8_t i = 0; i < num_outputs; ++i) { + motor_out[i] = (uint16_t)_outputs.output[i]; + } + + break; + } + + // 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; + } + + _outputs.timestamp = hrt_absolute_time(); + + send_esc_outputs(motor_out, num_outputs); + tap_esc_common::read_data_from_uart(_uart_fd, &_uartbuf); + + 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].timestamp = hrt_absolute_time(); + _esc_feedback.esc[feed_back_data.channelID].esc_errorcount = 0; + _esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed; +#if defined(ESC_HAVE_VOLTAGE_SENSOR) + _esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage; +#endif // ESC_HAVE_VOLTAGE_SENSOR +#if defined(ESC_HAVE_CURRENT_SENSOR) + _esc_feedback.esc[feed_back_data.channelID].esc_current = feed_back_data.current; +#endif // ESC_HAVE_CURRENT_SENSOR +#if defined(ESC_HAVE_TEMPERATURE_SENSOR) + _esc_feedback.esc[feed_back_data.channelID].esc_temperature = feed_back_data.temperature; +#endif // ESC_HAVE_TEMPERATURE_SENSOR + _esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus; + _esc_feedback.esc[feed_back_data.channelID].failures = 0; + //_esc_feedback.esc[feed_back_data.channelID].esc_setpoint_raw = motor_out[feed_back_data.channelID]; + //_esc_feedback.esc[feed_back_data.channelID].esc_setpoint = (float)motor_out[feed_back_data.channelID] * 15.13f - 16171.4f; + + _esc_feedback.counter++; + _esc_feedback.esc_count = num_outputs; + _esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + + _esc_feedback.esc_online_flags |= 1 << feed_back_data.channelID; + _esc_feedback.esc_armed_flags |= 1 << feed_back_data.channelID; + + _esc_feedback.timestamp = hrt_absolute_time(); + _esc_feedback_pub.publish(_esc_feedback); + } + } + } + + /* and publish for anyone that cares to see */ + _outputs_pub.publish(_outputs); + + // use first valid timestamp_sample for latency tracking + for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + const bool required = _groups_required & (1 << i); + const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; + + if (required && (timestamp_sample > 0)) { + perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample); + break; + } + } + } + + if (_armed_sub.updated()) { + _armed_sub.copy(&_armed); + + if (_is_armed != _armed.armed) { + /* reset all outputs */ + for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) { + _outputs.output[i] = NAN; + } + } + + _is_armed = _armed.armed; + } + + // Handle tunes + if (_tune_control_sub.updated()) { + tune_control_s tune; + + if (_tune_control_sub.copy(&tune)) { + if (tune.timestamp > 0) { + Tunes::ControlResult result = _tunes.set_control(tune); + _play_tone = (result == Tunes::ControlResult::Success) || (result == Tunes::ControlResult::AlreadyPlaying); + PX4_DEBUG("new tune id: %d, result: %d, play: %d", tune.tune_id, (int)result, _play_tone); + } + } + } + + const hrt_abstime timestamp_now = hrt_absolute_time(); + + if ((timestamp_now - _interval_timestamp <= _duration) || !_play_tone) { + //return; + } else { + _interval_timestamp = timestamp_now; + + if (_silence_length > 0) { + _duration = _silence_length; + _silence_length = 0; + + } else if (_play_tone) { + uint8_t strength = 0; + Tunes::Status parse_ret_val = _tunes.get_next_note(_frequency, _duration, _silence_length, strength); + + if (parse_ret_val == Tunes::Status::Continue) { + // Continue playing. + _play_tone = true; + + if (_frequency > 0) { + // Start playing the note. + EscbusTunePacket esc_tune_packet{}; + esc_tune_packet.frequency = _frequency; + esc_tune_packet.duration_ms = (uint16_t)(_duration / 1000); // convert to ms + esc_tune_packet.strength = strength; + send_tune_packet(esc_tune_packet); + } + + } else { + _play_tone = false; + _silence_length = 0; + } + } + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + +int TAP_ESC::control_callback_trampoline(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) +{ + TAP_ESC *obj = (TAP_ESC *)handle; + return obj->control_callback(control_group, control_index, input); +} + +int TAP_ESC::control_callback(uint8_t control_group, uint8_t control_index, float &input) +{ + input = _controls[control_group].control[control_index]; + + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + + } else if (input < -1.0f) { + input = -1.0f; + } + + /* throttle not arming - mark throttle input as invalid */ + if (_armed.prearmed && !_armed.armed) { + 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) { + /* set the throttle to an invalid value */ + input = NAN; + } + } + + return 0; +} + +int TAP_ESC::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + + if (_mixers == nullptr) { + _mixers = new MixerGroup(); + } + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + ret = _mixers->load_from_buf(control_callback_trampoline, (uintptr_t)this, buf, buflen); + + if (ret != 0) { + PX4_DEBUG("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + + } else { + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +/** @see ModuleBase */ +void TAP_ESC::run() +{ + // Main loop + while (!should_exit()) { + cycle(); + } +} + +/** @see ModuleBase */ +int TAP_ESC::task_spawn(int argc, char *argv[]) +{ + /* start the task */ + _task_id = px4_task_spawn_cmd("tap_esc", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1472, + (px4_main_t)&run_trampoline, argv); + + if (_task_id < 0) { + PX4_ERR("task start failed"); + _task_id = -1; + return PX4_ERROR; + } + + // wait until task is up & running + if (wait_until_running() < 0) { + _task_id = -1; + return -1; + } + + return PX4_OK; +} + +/** @see ModuleBase */ +int TAP_ESC::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + 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. + +### 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. + +### Example +The module is typically started with: +tap_esc start -d /dev/ttyS2 -n <1-8> + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("tap_esc", "driver"); + + 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; +} + +extern "C" __EXPORT int tap_esc_main(int argc, char *argv[]) +{ + return TAP_ESC::main(argc, argv); +} diff --git a/src/drivers/tap_esc/TAP_ESC.hpp b/src/drivers/tap_esc/TAP_ESC.hpp new file mode 100644 index 0000000000..e4bbe41871 --- /dev/null +++ b/src/drivers/tap_esc/TAP_ESC.hpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include // NAN +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "tap_esc_common.h" + +#include "drv_tap_esc.h" + +#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 + +// uorb update rate for control groups in milliseconds +#if !defined(TAP_ESC_CTRL_UORB_UPDATE_INTERVAL) +# define TAP_ESC_CTRL_UORB_UPDATE_INTERVAL 2 // [ms] min: 2, max: 100 +#endif + +/* + * This driver connects to TAP ESCs via serial. + */ +class TAP_ESC : public cdev::CDev, public ModuleBase, public ModuleParams +{ +public: + TAP_ESC(char const *const device, uint8_t channels_count); + virtual ~TAP_ESC(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static TAP_ESC *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + virtual int init(); + virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + void cycle(); + +private: + + void subscribe(); + void send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt); + inline void send_tune_packet(EscbusTunePacket &tune_packet); + 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); + + 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}; + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _test_motor_sub{ORB_ID(test_motor)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; + actuator_outputs_s _outputs{}; + actuator_armed_s _armed{}; + + perf_counter_t _perf_control_latency{perf_alloc(PC_ELAPSED, MODULE_NAME": control latency")}; + + 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}; + + uORB::PublicationMulti _esc_feedback_pub{ORB_ID(esc_status)}; + uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags + esc_status_s _esc_feedback{}; + uint8_t _channels_count{0}; ///< number 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{}; + + Tunes _tunes{}; + uORB::Subscription _tune_control_sub{ORB_ID(tune_control)}; + hrt_abstime _interval_timestamp{0}; + bool _play_tone{false}; + unsigned int _silence_length{0}; ///< If nonzero, silence before next note. + unsigned int _frequency{0}; + unsigned int _duration{0}; + + LedControlData _led_control_data{}; + LedController _led_controller{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< multicopter air-mode + ) +}; diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h new file mode 100644 index 0000000000..041bff1854 --- /dev/null +++ b/src/drivers/tap_esc/drv_tap_esc.h @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#define TAP_ESC_DEVICE_PATH "/dev/tap_esc" + +/* 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 + +#define PACKET_ID_MASK 0x55 + +#define NO_ESC_ID_CONFIG 0x0f + +/* 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 + * .... + * + */ + +// Circular from back right in CCW direction +#define ESC_POS {0, 1, 2, 3, 4, 5, 6, 7} +// 0 is CW, 1 is CCW +#define ESC_DIR {0, 1, 0, 1, 1 ,1, 1, 1} + +#define RPMMAX 1900 +#define RPMMIN 1200 +#define RPMSTOPPED (RPMMIN - 10) + + +#define MAX_BOOT_TIME_MS (550) // Minimum time to wait after Power on before sending commands + +#pragma pack(push,1) + +/****** 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_MASK_MAP_CHANNEL 0x0f +#define ESC_MASK_MAP_RUNNING_DIRECTION 0xf0 +/****** ConFigInfoBasicResponse ***********/ + +/****** InfoRequest ***********/ +typedef enum { + REQUEST_INFO_BASIC = 0, + REQUEST_INFO_FUll, + REQUEST_INFO_RUN, + REQUEST_INFO_STUDY, + REQUEST_INFO_COMM, + REQUEST_INFO_DEVICE, +} InfoTypes; + +typedef struct { + uint8_t channelID; + uint8_t requestInfoType; +} InfoRequest; + +typedef struct { + uint16_t frequency; // 0 - 20kHz + uint16_t duration_ms; + uint8_t strength; +} EscbusTunePacket; + +/****** InfoRequest ***********/ + +/****** IdDoCmd ***********/ +// the real packet definition for ESCBUS_MSG_ID_DO_CMD +// command definition +typedef enum { + DO_RESET = 0, + DO_STUDY, + DO_ID_ASSIGNMENT, + DO_POWER_TEST, +} ESCBUS_ENUM_COMMAND; + +typedef struct { + uint8_t channelIDMask; + uint8_t command; + uint8_t escID; +} EscbusDoCmdPacket; + +typedef struct { + uint8_t id_mask; + uint8_t child_cmd; + uint8_t id; +} EscbusConfigidPacket; + +typedef struct { + uint8_t escID; +} AssignedIdResponse; +/****** IdDoCmd ***********/ + +/****** InfoRequest ***********/ + +typedef struct { + uint8_t head; + uint8_t len; + uint8_t msg_id; + union { + InfoRequest reqInfo; + ConfigInfoBasicRequest reqConfigInfoBasic; + RunReq reqRun; + EscbusTunePacket tunePacket; + EscbusConfigidPacket configidPacket; + ConfigInfoBasicResponse rspConfigInfoBasic; + RunInfoRepsonse rspRunInfo; + AssignedIdResponse rspAssignedId; + uint8_t bytes[100]; + } d; + uint8_t crc_data; + +} EscPacket; + +#define UART_BUFFER_SIZE 128 +typedef struct { + uint8_t head; + uint8_t tail; + uint8_t dat_cnt; + uint8_t esc_feedback_buf[UART_BUFFER_SIZE]; +} ESC_UART_BUF; + +#pragma pack(pop) +/****************************************************************************************** + * ESCBUS_MSG_ID_RUN_INFO packet + * + * Monitor message of ESCs while motor is running + * + * channelID: assigned channel number + * + * ESCStatus: status of ESC + * Num Health status + * 0 HEALTHY + * 1 WARNING_LOW_VOLTAGE + * 2 WARNING_OVER_CURRENT + * 3 WARNING_OVER_HEAT + * 4 ERROR_MOTOR_LOW_SPEED_LOSE_STEP + * 5 ERROR_MOTOR_STALL + * 6 ERROR_HARDWARE + * 7 ERROR_LOSE_PROPELLER + * 8 ERROR_OVER_CURRENT + * + * speed: -32767 - 32767 rpm + * + * temperature: 0 - 256 celsius degree (if available) + * voltage: 0.00 - 100.00 V (if available) + * current: 0.0 - 200.0 A (if available) + */ + +typedef enum { + ESC_STATUS_HEALTHY, + ESC_STATUS_WARNING_LOW_VOLTAGE, + ESC_STATUS_WARNING_OVER_HEAT, + ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP, + ESC_STATUS_ERROR_MOTOR_STALL, + 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; + + +typedef enum { + // messages or command to ESC + ESCBUS_MSG_ID_CONFIG_BASIC = 0, + ESCBUS_MSG_ID_CONFIG_FULL, + ESCBUS_MSG_ID_RUN, + ESCBUS_MSG_ID_TUNE, + ESCBUS_MSG_ID_DO_CMD, + // messages from ESC + ESCBUS_MSG_ID_REQUEST_INFO, + ESCBUS_MSG_ID_CONFIG_INFO_BASIC, // simple configuration info for request from flight controller + ESCBUS_MSG_ID_CONFIG_INFO_FULL,// full configuration info for request from host such as computer + ESCBUS_MSG_ID_RUN_INFO,// feedback message in RUN mode + ESCBUS_MSG_ID_STUDY_INFO, // studied parameters in STUDY mode + ESCBUS_MSG_ID_COMM_INFO, // communication method info + ESCBUS_MSG_ID_DEVICE_INFO,// ESC device info + ESCBUS_MSG_ID_ASSIGNED_ID, // never touch ESCBUS_MSG_ID_MAX_NUM + // bootloader used + PROTO_OK = 0x10, // INSYNC/OK - 'ok' response + PROTO_FAILED = 0x11, // INSYNC/FAILED - 'fail' response + + ESCBUS_MSG_ID_BOOT_SYNC = 0x21, // boot loader used + PROTO_GET_DEVICE = 0x22, // get device ID bytes + PROTO_CHIP_ERASE = 0x23, // erase program area and reset program address + PROTO_PROG_MULTI = 0x27, // write bytes at program address and increment + PROTO_GET_CRC = 0x29, // compute & return a CRC + PROTO_BOOT = 0x30, // boot the application + PROTO_GET_SOFTWARE_VERSION = 0x40, + ESCBUS_MSG_ID_MAX_NUM, +} +ESCBUS_ENUM_MESSAGE_ID; + +typedef enum { + HEAD, + LEN, + ID, + DATA, + CRC, + +} PARSR_ESC_STATE; diff --git a/src/drivers/tap_esc/tap_esc_common.cpp b/src/drivers/tap_esc/tap_esc_common.cpp new file mode 100644 index 0000000000..d2ba932dbc --- /dev/null +++ b/src/drivers/tap_esc/tap_esc_common.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * tap_esc_common.cpp + * + */ + +#include "tap_esc_common.h" + +#include +#include + +#include // arraySize +#include +#include + +#ifndef B250000 +#define B250000 250000 +#endif + +#if defined(GPIO_CAN1_SILENT_S0) +# define ESC_MUX_SELECT0 GPIO_CAN1_SILENT_S0 +# define ESC_MUX_SELECT1 GPIO_CAN2_SILENT_S1 +# define ESC_MUX_SELECT2 GPIO_CAN3_SILENT_S2 +#endif + +namespace tap_esc_common +{ +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(ESC_MUX_SELECT0) + px4_arch_gpiowrite(ESC_MUX_SELECT0, sel & 1); + px4_arch_gpiowrite(ESC_MUX_SELECT1, sel & 2); + px4_arch_gpiowrite(ESC_MUX_SELECT2, sel & 4); +#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, + 0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77, + 0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81, + 0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C, + 0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09, + 0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4, + 0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02, + 0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46, + 0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F, + 0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03, + 0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5, + 0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08, + 0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8, + 0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15, + 0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3, + 0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32, + 0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B, + 0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F, + 0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9, + 0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24, + 0xB8, 0x5F, 0x91, 0x76 +}; + +} /* tap_esc_common */ diff --git a/src/drivers/tap_esc/tap_esc_common.h b/src/drivers/tap_esc/tap_esc_common.h new file mode 100644 index 0000000000..922c6bbd7b --- /dev/null +++ b/src/drivers/tap_esc/tap_esc_common.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * tap_esc_common.h + * + */ + +#pragma once + +#include + +#include "drv_tap_esc.h" + +namespace tap_esc_common +{ +/** + * 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 */ diff --git a/src/drivers/tap_esc/tap_esc_params.c b/src/drivers/tap_esc/tap_esc_params.c new file mode 100644 index 0000000000..fae32a918e --- /dev/null +++ b/src/drivers/tap_esc/tap_esc_params.c @@ -0,0 +1,32 @@ +/** + * @file tap_esc_params.c + * Parameters for esc version + * + */ + +/** + * Required esc firmware version. + * + * @group ESC + * @min 0 + * @max 65535 + */ +PARAM_DEFINE_INT32(ESC_FW_VER, 0); + +/** + * Required esc bootloader version. + * + * @group ESC + * @min 0 + * @max 65535 + */ +PARAM_DEFINE_INT32(ESC_BL_VER, 0); + +/** + * Required esc hardware version + * + * @group ESC + * @min 0 + * @max 65535 + */ +PARAM_DEFINE_INT32(ESC_HW_VER, 0); diff --git a/src/drivers/tap_esc/tap_esc_uploader.cpp b/src/drivers/tap_esc/tap_esc_uploader.cpp new file mode 100644 index 0000000000..153fcd081a --- /dev/null +++ b/src/drivers/tap_esc/tap_esc_uploader.cpp @@ -0,0 +1,1356 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tap_esc_uploader.cpp + * firmware uploader for TAP ESC + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drv_tap_esc.h" +#include "tap_esc_uploader.h" +#include "tap_esc_common.h" + +// define for comms logging +//#define UDEBUG + +const uint8_t TAP_ESC_UPLOADER::_device_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS; + +TAP_ESC_UPLOADER::TAP_ESC_UPLOADER(const char *device, uint8_t esc_counter) : + _device(device), + _esc_counter(esc_counter) +{ +} + +TAP_ESC_UPLOADER::~TAP_ESC_UPLOADER() +{ + tap_esc_common::deinitialise_uart(_esc_fd); +} + +int32_t TAP_ESC_UPLOADER::initialise_firmware_file(const char *filenames[], int &fw_fd) +{ + const char *filename = NULL; + + /* allow an early abort and look for file first */ + for (unsigned i = 0; filenames[i] != nullptr; i++) { + fw_fd = open(filenames[i], O_RDONLY); + + if (fw_fd < 0) { + PX4_DEBUG("failed to open %s", filenames[i]); + continue; + } + + PX4_DEBUG("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + PX4_DEBUG("no firmware found"); + return -ENOENT; + } + + struct stat st; + + if (stat(filename, &st) != 0) { + PX4_DEBUG("Failed to stat %s - %d\n", filename, (int)errno); + return -errno; + } + + if (fw_fd == -1) { + return -ENOENT; + } + + return st.st_size; +} + +int TAP_ESC_UPLOADER::upload_id(uint8_t esc_id, int32_t fw_size) +{ + int ret = -1; + + /****************************************** + * first:send sync + ******************************************/ + PX4_DEBUG("uploader esc_id %d...", esc_id); + + /* look for the bootloader, blocking 320 ms */ + for (int i = 0; i < SYNC_RETRY_TIMES; i++) { + ret = sync(esc_id); + + if (ret == OK) { + break; + + } else { + /* send sync interval time: 50ms */ + usleep(50000); + } + } + + if (ret != OK) { + /* this is immediately fatal */ + PX4_DEBUG("esc_id %d bootloader not responding", esc_id); + return -EIO; + } + + /* do the usual program thing - allow for failure */ + for (unsigned retries = 0; retries < UPLOADER_RETRY_TIMES; retries++) { + if (retries > 0) { + PX4_DEBUG("esc_id %d retrying update...", esc_id); + ret = sync(esc_id); + + if (ret != OK) { + /* this is immediately fatal */ + PX4_DEBUG("esc_id %d bootloader not responding", esc_id); + return -EIO; + } + } + + /****************************************** + * second: get device bootloader revision + ******************************************/ + uint32_t bl_rev; + ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_BL_REV, bl_rev); + + if (ret == OK) { + if (bl_rev <= PROTO_SUPPORT_BL_REV * 100) { + PX4_DEBUG("esc_id %d found bootloader revision: %4.4f", esc_id, (double)bl_rev / 100); + + } else { + PX4_DEBUG("esc_id %d found unsupported bootloader revision %4.4f, exiting", esc_id, (double)bl_rev / 100); + return EPERM; + } + + } else { + PX4_DEBUG("esc_id %d found bootloader revision failed", esc_id); + } + + /****************************************** + * third: erase program + ******************************************/ + ret = erase(esc_id); + + if (ret != OK) { + PX4_DEBUG("esc_id %d %d erase failed", esc_id, ret); + continue; + } + + /****************************************** + * fourth: program + ******************************************/ + ret = program(esc_id, fw_size); + + if (ret != OK) { + PX4_DEBUG("esc_id %d program failed", esc_id); + continue; + } + + /****************************************** + * fifth: verify flash crc + *****************************************/ + ret = verify_crc(esc_id, fw_size); + + if (ret != OK) { + PX4_DEBUG("verify failed"); + continue; + } + + /****************************************** + * sixth: reboot tap esc + *****************************************/ + ret = reboot(esc_id); + + if (ret != OK) { + PX4_DEBUG("reboot failed,reboot again"); + ret = reboot(esc_id); + usleep(2000); + + if (ret != OK) { + PX4_DEBUG("reboot failed"); + return ret; + } + } + + PX4_DEBUG("esc_id %d uploader complete", esc_id); + + ret = OK; + break; + } + + return OK; +} + +int TAP_ESC_UPLOADER::upload(const char *filenames[]) +{ + int ret = -1; + int32_t fw_size; + uint16_t esc_fail_mask = 0; + + fw_size = initialise_firmware_file(filenames, _fw_fd); + + if (fw_size < 0) { + PX4_DEBUG("initialise firmware file failed"); + return fw_size; + } + + ret = tap_esc_common::initialise_uart(_device, _esc_fd); + + if (ret < 0) { + PX4_DEBUG("initialise uart failed %s"); + return ret; + } + + /* uploader esc_id(0,1,2,3,4,5), uploader begin esc id0 */ + for (unsigned esc_id = 0; esc_id < _esc_counter; esc_id++) { + + ret = upload_id(esc_id, fw_size); + + if (ret != OK) { + esc_fail_mask = 1 << esc_id; + } + } + + // check all esc upload success after all esc uploading end,it will upload esc again which esc upload failed + for (unsigned esc_id = 0; esc_id < _esc_counter; esc_id++) { + + if (esc_fail_mask & (1 << esc_id)) { + ret = upload_id(esc_id, fw_size); + } + } + + esc_fail_mask = 0; + + // sleep for enough time for the TAP ESC chip to boot. This makes + // update more reliably startup TAP ESC again after upload + // set wait time for tap esc form reboot jump app(0.1269s measure by Saleae logic Analyzer) + usleep(130 * 1000); + + return ret; +} + +int TAP_ESC_UPLOADER::log_versions() +{ + param_t param_esc_fw_ver = param_find("ESC_FW_VER"); + param_t param_esc_hw_ver = param_find("ESC_HW_VER"); + param_t param_esc_bl_ver = param_find("ESC_BL_VER"); + + if (param_esc_fw_ver == PARAM_INVALID || + param_esc_hw_ver == PARAM_INVALID || + param_esc_bl_ver == PARAM_INVALID) { + PX4_WARN("Cannot find one or more parameters, unable to log ESC versions."); + return -1; + } + + int ret = tap_esc_common::initialise_uart(_device, _esc_fd); + + if (ret < 0) { + PX4_DEBUG("initialise uart failed %s"); + return ret; + } + + // get information from first ESC + uint32_t fw_ver, hw_ver, bl_ver; + ret = get_esc_versions(0, fw_ver, hw_ver, bl_ver); + + if (ret != OK) { + PX4_DEBUG("Failed to get ESC 0 device info"); + return ret; + } + + /* Get firmware versions of the remainig ESCs and compare with the first one + Since the parameters can only store one version (not six), we need to make + sure that all ESCs have matchin version numbers. If not, all version + parameters will be set to zero. */ + bool esc_versions_matching = true; + + for (unsigned esc_id = 1; esc_id < _esc_counter; esc_id++) { + uint32_t temp_fw_ver, temp_hw_ver, temp_bl_ver; + ret = get_esc_versions(esc_id, temp_fw_ver, temp_hw_ver, temp_bl_ver); + + if (ret != OK) { + PX4_DEBUG("Failed to get ESC %u device info", esc_id); + return ret; + } + + if (fw_ver != temp_fw_ver || hw_ver != temp_hw_ver || bl_ver != temp_bl_ver) { + esc_versions_matching = false; + break; + } + } + + if (!esc_versions_matching) { + // One or more ESCs have mismatching versions + fw_ver = 0; // version 0 means unknown + hw_ver = 0; // version 0 means unknown + bl_ver = 0; // version 0 means unknown + } + + param_set(param_esc_fw_ver, &fw_ver); + param_set(param_esc_hw_ver, &hw_ver); + param_set(param_esc_bl_ver, &bl_ver); + + return ret; +} + +int TAP_ESC_UPLOADER::checkcrc(const char *filenames[]) +{ + /* + check tap_esc flash CRC against CRC of a file + */ + int32_t fw_size; + int ret = -1; + uint16_t esc_fail_mask = 0; + + fw_size = initialise_firmware_file(filenames, _fw_fd); + + if (fw_size < 0) { + PX4_ERR("initialise firmware file failed"); + return fw_size; + } + + ret = tap_esc_common::initialise_uart(_device, _esc_fd); + + if (ret < 0) { + PX4_DEBUG("initialise uart failed %s"); + return ret; + } + + /* checkcrc esc_id(0,1,2,3,4,5), checkcrc begin esc id0 */ + for (unsigned esc_id = 0; esc_id < _esc_counter; esc_id++) { + /* look for the bootloader, blocking 320 ms,uploader begin esc id0*/ + for (int i = 0; i < SYNC_RETRY_TIMES; i++) { + ret = sync(esc_id); + + if (ret == OK) { + break; + + } else { + /* send sync interval time: 50ms */ + usleep(50000); + } + } + + if (ret != OK) { + /* this is immediately fatal */ + PX4_DEBUG("esc_id %d bootloader not responding", esc_id); + return -EIO; + } + + uint32_t temp_revision; + + /* get device bootloader revision */ + ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_BL_REV, temp_revision); + + double test_esc_id; + test_esc_id = esc_id; + + if (ret == OK) { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found bootloader revision: %4.4f", test_esc_id, + (double)temp_revision / 100); + + } else { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found bootloader revision failed", test_esc_id); + } + + /* get device firmware revision */ + ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_FW_REV, temp_revision); + + if (ret == OK) { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found firmware revision: %4.4f", test_esc_id, + (double)temp_revision / 100); + + } else { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found firmware revision failed", test_esc_id); + } + + /* get device hardware revision */ + ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_BOARD_REV, temp_revision); + + if (ret == OK) { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found board revision: %02x", test_esc_id, temp_revision); + + } else { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f found board revision failed", test_esc_id); + } + + /* compare esc flash crc with .bin file crc */ + ret = verify_crc(esc_id, fw_size); + + if (ret == -EINVAL) { + mavlink_log_info(&_mavlink_log_pub, "esc_id %1.f check CRC is different,will upload tap esc firmware ", + test_esc_id); + ret = upload_id(esc_id, fw_size); + + if (ret != OK) { + esc_fail_mask = 1 << esc_id; + } + + } else { + /* reboot tap esc_id */ + ret = reboot(esc_id); + + if (ret != OK) { + PX4_DEBUG("reboot failed,reboot again"); + ret = reboot(esc_id); + usleep(2000); + + if (ret != OK) { + PX4_DEBUG("reboot failed"); + return ret; + } + } + } + } + + // check all esc upload success after all esc uploading end,it will upload esc again which esc upload failed + for (unsigned esc_id = 0; esc_id < _esc_counter; esc_id++) { + + if (esc_fail_mask & (1 << esc_id)) { + ret = upload_id(esc_id, fw_size); + } + } + + esc_fail_mask = 0; + + // sleep for enough time for the TAP ESC chip to boot. This makes + // update more reliably startup TAP ESC again after upload + // set wait time for tap esc form rebbot jump app(0.1269s measure by Saleae logic Analyzer) + usleep(130 * 1000); + + return ret; +} + +int TAP_ESC_UPLOADER::recv_byte_with_timeout(uint8_t *c, unsigned timeout) +{ + struct pollfd fds[1]; + + fds[0].fd = _esc_fd; + fds[0].events = POLLIN; + + /* wait ms for a character */ + int ret = ::poll(&fds[0], 1, timeout); + + if (ret < 1) { +#ifdef UDEBUG + PX4_DEBUG("poll timeout %d", ret); +#endif + return -ETIMEDOUT; + } + + read(_esc_fd, c, 1); +#ifdef UDEBUG + PX4_DEBUG("recv_bytes 0x%02x", c); +#endif + return OK; +} + +int TAP_ESC_UPLOADER::read_and_parse_data(unsigned timeout) +{ + uint8_t c = 0; + int ret = -1; + + /* set a timeout Ms for a first byte */ + ret = recv_byte_with_timeout(&c, timeout); + + if (ret == OK) { + do { + /* try to parse the message from esc */ + ret = parse_tap_esc_feedback(c, &_uploader_packet); + + /* parse success all data */ + if (ret == OK) { + return OK; + } + + /* read a byte and buffer it with poll and a timeout 2Ms */ + ret = recv_byte_with_timeout(&c, 2); + + /* timeout in 1 is exceeded */ + if (ret == -ETIMEDOUT) { + PX4_DEBUG("parse timeout %d data %d", ret, c); + return ret; + } + + } while (true); + + } + + return ret; +} + +int TAP_ESC_UPLOADER::parse_tap_esc_feedback(uint8_t decode_data, EscUploaderMessage *packetdata) +{ + static PARSR_ESC_STATE state = HEAD; + static uint8_t data_index = 0; + static uint8_t crc_data_cal; + +#ifdef UDEBUG + PX4_DEBUG("decode data 0x%02x", decode_data); +#endif + + switch (state) { + case HEAD: + if (decode_data == PACKET_HEAD) { + packetdata->head = PACKET_HEAD; //just_keep the format + state = LEN; + + } + + break; + + case LEN: + if (decode_data < sizeof(packetdata->d)) { + packetdata->len = decode_data; + state = ID; + + } else { + state = HEAD; + } + + break; + + case ID: + if (decode_data < PROTO_MSG_ID_MAX_NUM) { + packetdata->msg_id = decode_data; + data_index = 0; + state = DATA; + + } else { + state = HEAD; + } + + break; + + case DATA: + packetdata->d.data[data_index++] = decode_data; + + 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 == decode_data) { + packetdata->crc_data = decode_data; + state = HEAD; + return OK; + + } + + state = HEAD; + break; + + default: + state = HEAD; + break; + + } + + return EPROTO; +} + +uint8_t TAP_ESC_UPLOADER::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_UPLOADER::crc_packet(EscUploaderMessage &p) +{ + /* Calculate the crc include Len,ID,data */ + p.d.data[p.len] = crc8_esc(&p.len, p.len + 2); + return p.len + offsetof(EscUploaderMessage, d) + 1; +} + +int TAP_ESC_UPLOADER::send_packet(EscUploaderMessage &packet, int responder) +{ + if (responder >= 0) { + + if (responder > _esc_counter) { + return -EINVAL; + } + + /* For messages with id ESCBUS_MSG_ID_REQUEST_INFO we need to send + the channel ID, not the ESC hardware ID */ + int esc_select_id = ((packet.msg_id == ESCBUS_MSG_ID_REQUEST_INFO) ? responder : _device_mux_map[responder]); + tap_esc_common::select_responder(esc_select_id); + } + + int packet_len = crc_packet(packet); + int ret = ::write(_esc_fd, (uint8_t *)&packet.head, packet_len); + +#ifdef UDEBUG + uint8_t *buf = (uint8_t *)&packet; + + for (int i = 0; i < packet_len; i++) { + PX4_DEBUG("buf[%d] 0x%02x %d", i, buf[i], ret); + } + +#endif + + if (ret != packet_len) { +#ifdef UDEBUG + PX4_DEBUG("TX ERROR: ret: %d, errno: %d", ret, errno); +#endif + return errno; + } + + return OK; +} + +int TAP_ESC_UPLOADER::sync(uint8_t esc_id) +{ + /* send sync packet */ + EscUploaderMessage sync_packet{PACKET_HEAD, sizeof(EscbusBootSyncPacket), PROTO_GET_SYNC}; + sync_packet.d.sync_packet.myID = esc_id; + send_packet(sync_packet, esc_id); + + /* read and parse sync feedback packet, blocking 320ms between esc app jump boot,but if esc run boot,it waits time less than 2ms */ + /* set wait time for tap esc form app jump reboot(0.311748s measure by Saleae logic Analyzer) */ + int ret = read_and_parse_data(320); + + if (ret != OK) { + return ret; + } + + /* check sync feedback is ok or fail */ + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.feedback_packet.myID != esc_id) { + PX4_DEBUG("sync don't match myID: 0x%02x, esc_id: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id); + return -EIO; + } + + if (_uploader_packet.d.feedback_packet.command != PROTO_GET_SYNC) { + PX4_DEBUG("bad sync myID: 0x%02x, command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("sync invalid: don't receive sync invalid: myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.feedback_packet.myID, + esc_id); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("sync failed: don't receive sync failed: myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.feedback_packet.myID, + esc_id); + return -EIO; + + } + + return OK; +} + +// TODO: It is weird, that we have a separate function to request the ESC +// versions when there is also the get_device_info() function. Problem is, that +// the versions request will return three values instead of only one, hence +// the need for this new function here. Ideally we would utilize +// get_device_info() for all requests... +int TAP_ESC_UPLOADER::get_esc_versions(uint8_t esc_id, uint32_t &fw_ver, uint32_t &hw_ver, uint32_t &bl_ver) +{ + /* prepare information request packet */ + EscUploaderMessage device_info_packet = {PACKET_HEAD, sizeof(EscbusGetDevicePacket), ESCBUS_MSG_ID_REQUEST_INFO}; + device_info_packet.d.device_info_packet.myID = esc_id; + device_info_packet.d.device_info_packet.deviceInfo = REQUEST_INFO_DEVICE; + send_packet(device_info_packet, esc_id); + + /* read and parse device information feedback packet, blocking 50ms */ + int ret = read_and_parse_data(); + + if (ret != OK) { + return ret; + } + + /* check device information feedback is ok or fail */ + if (_uploader_packet.msg_id == ESCBUS_MSG_ID_DEVICE_INFO) { + if (_uploader_packet.d.esc_version_packet.myID != esc_id) { + PX4_DEBUG("get device firmware revision: ID mismatch, received: 0x%02x, asked for: 0x%02x", + _uploader_packet.d.esc_version_packet.myID, esc_id); + return -EIO; + } + + fw_ver = _uploader_packet.d.esc_version_packet.FwRev; + hw_ver = _uploader_packet.d.esc_version_packet.HwRev; + bl_ver = _uploader_packet.d.esc_version_packet.blRev; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("Failed to get ESC versions, ESC ID: 0x%02x, we sent to ID: 0x%02x, FwRev: 0x%02x", + _uploader_packet.d.esc_version_packet.myID, esc_id, + _uploader_packet.d.esc_version_packet.FwRev); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("Failed to get ESC versions, ESC reports invalid protocl, ESC ID: 0x%02x, we sent to ID: 0x%02x, FwRev: 0x%02x", + _uploader_packet.d.esc_version_packet.myID, esc_id, + _uploader_packet.d.esc_version_packet.FwRev); + return -EIO; + + } + + return OK; +} + +int TAP_ESC_UPLOADER::get_device_info(uint8_t esc_id, uint8_t msg_id, uint8_t msg_arg, uint32_t &val) +{ + /* prepare information request packet */ + EscUploaderMessage device_info_packet{PACKET_HEAD, sizeof(EscbusGetDevicePacket), msg_id}; + device_info_packet.d.device_info_packet.myID = esc_id; + device_info_packet.d.device_info_packet.deviceInfo = msg_arg; + send_packet(device_info_packet, esc_id); + + /* read and parse device information feedback packet, blocking 50ms */ + int ret = read_and_parse_data(); + + if (ret != OK) { + return ret; + } + + /* check device information feedback is ok or fail */ + switch (msg_arg) { + case PROTO_DEVICE_BL_REV: + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.bootloader_revis_packet.myID != esc_id) { + PX4_DEBUG("get device bootloader revision id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.bootloader_revis_packet.myID, esc_id); + return -EIO; + } + + val = _uploader_packet.d.bootloader_revis_packet.version; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("get device bootloader revision failed, myID: 0x%02x, esc_id: 0x%02x, ver: 0x%02x", + _uploader_packet.d.bootloader_revis_packet.myID, esc_id, + _uploader_packet.d.bootloader_revis_packet.version); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("get device bootloader revision invalid, myID: 0x%02x, esc_id: 0x%02x, ver: 0x%02x", + _uploader_packet.d.bootloader_revis_packet.myID, esc_id, + _uploader_packet.d.bootloader_revis_packet.version); + return -EIO; + + } + + break; + + case PROTO_DEVICE_BOARD_ID: + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.hardware_id_packet.myID != esc_id) { + PX4_DEBUG("get device hardware_id id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.hardware_id_packet.myID, + esc_id); + return -EIO; + } + + val = _uploader_packet.d.hardware_id_packet.targetSystemId; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("get device hardware id failed, myID: 0x%02x, esc_id: 0x%02x, tar: 0x%02x, tar: 0x%02x", + _uploader_packet.d.hardware_id_packet.myID, esc_id, + _uploader_packet.d.hardware_id_packet.targetSystemId); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("get device hardware id invalid, myID: 0x%02x, esc_id: 0x%02x, tar: 0x%02x, tar: 0x%02x", + _uploader_packet.d.hardware_id_packet.myID, esc_id, + _uploader_packet.d.hardware_id_packet.targetSystemId); + return -EIO; + + } + + break; + + case PROTO_DEVICE_BOARD_REV: + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.hardware_revis_packet.myID != esc_id) { + PX4_DEBUG("get device hardware revision id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.hardware_revis_packet.myID, esc_id); + return -EIO; + } + + val = _uploader_packet.d.hardware_revis_packet.boardRev; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("get device hardware revision failed, myID: 0x%02x, esc_id: 0x%02x, boardRev: 0x%02x", + _uploader_packet.d.hardware_revis_packet.myID, esc_id, + _uploader_packet.d.hardware_revis_packet.boardRev); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("get device hardware revision invalid, myID: 0x%02x, esc_id: 0x%02x, boardRev: 0x%02x", + _uploader_packet.d.hardware_revis_packet.myID, esc_id, + _uploader_packet.d.hardware_revis_packet.boardRev); + return -EIO; + + } + + break; + + case PROTO_DEVICE_FW_SIZE: + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.firmware_size_packet.myID != esc_id) { + PX4_DEBUG("get device firmware size id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.firmware_size_packet.myID, esc_id); + return -EIO; + } + + val = _uploader_packet.d.firmware_size_packet.FwSize; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("get device firmware size failed, myID: 0x%02x, esc_id: 0x%02x, FwSize: 0x%02x", + _uploader_packet.d.firmware_size_packet.myID, esc_id, + _uploader_packet.d.firmware_size_packet.FwSize); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("get device firmware size invalid, myID: 0x%02x, esc_id: 0x%02x, FwSize: 0x%02x", + _uploader_packet.d.firmware_size_packet.myID, esc_id, + _uploader_packet.d.firmware_size_packet.FwSize); + return -EIO; + + } + + break; + + case REQUEST_INFO_DEVICE: + if (_uploader_packet.msg_id == ESCBUS_MSG_ID_DEVICE_INFO) { + if (_uploader_packet.d.firmware_revis_packet.myID != esc_id) { + PX4_DEBUG("get device firmware revision id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.firmware_revis_packet.myID, esc_id); + return -EIO; + } + + val = _uploader_packet.d.firmware_revis_packet.FwRev; + + } + + break; + + case PROTO_DEVICE_FW_REV: + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.firmware_revis_packet.myID != esc_id) { + PX4_DEBUG("get device firmware revision id don't match, myID: 0x%02x, esc_id: 0x%02x", + _uploader_packet.d.firmware_revis_packet.myID, esc_id); + return -EIO; + } + + val = _uploader_packet.d.firmware_revis_packet.FwRev; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("get device firmware revision failed, myID: 0x%02x,esc_id: 0x%02x, FwRev: 0x%02x", + _uploader_packet.d.firmware_revis_packet.myID, esc_id, + _uploader_packet.d.firmware_revis_packet.FwRev); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("get device firmware revision invalid, myID: 0x%02x,esc_id: 0x%02x, FwRev: 0x%02x", + _uploader_packet.d.firmware_revis_packet.myID, esc_id, + _uploader_packet.d.firmware_revis_packet.FwRev); + return -EIO; + + } + + break; + + default: + break; + } + + return OK; +} + +int TAP_ESC_UPLOADER::erase(uint8_t esc_id) +{ + PX4_DEBUG("erase..."); + + /* send erase packet */ + EscUploaderMessage erase_packet{PACKET_HEAD, sizeof(EscbusBootErasePacket), PROTO_CHIP_ERASE}; + erase_packet.d.erase_packet.myID = esc_id; + send_packet(erase_packet, esc_id); + + /* read and parse erase feedback packet, blocking 500ms */ + int ret = read_and_parse_data(500); + + if (ret != OK) { + return ret; + } + + /* check erase feedback is ok or fail */ + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.feedback_packet.myID != esc_id) { + PX4_DEBUG("erase id don't match myID: 0x%02x,esc_id: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id); + return -EIO; + } + + if (_uploader_packet.d.feedback_packet.command != PROTO_CHIP_ERASE) { + PX4_DEBUG("erase bad command, myID: 0x%02x,command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("erase failed, myID: 0x%02x,esc_id: 0x%02x command: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("erase invalid, myID: 0x%02x, esc_id: 0x%02x command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } + + return OK; +} + +static int read_with_retry(int fd, void *buf, size_t n) +{ + int ret; + uint8_t retries = 0; + + do { + ret = read(fd, buf, n); + + } while (ret == -1 && retries++ < 100); + + if (retries != 0) { + PX4_WARN("read of %u bytes needed %u retries", (unsigned)n, + (unsigned)retries); + } + + return ret; +} + +int TAP_ESC_UPLOADER::program(uint8_t esc_id, size_t fw_size) +{ + ssize_t count = 0; + size_t sent = 0; + + uint8_t *file_buf = new uint8_t[PROG_MULTI_MAX]; + + if (!file_buf) { + PX4_DEBUG("Can't allocate program buffer"); + return -ENOMEM; + } + + //ASSERT((fw_size & 3) == 0); + //ASSERT((PROG_MULTI_MAX & 3) == 0); + + PX4_DEBUG("programming %u bytes...", (unsigned)fw_size); + + int ret = lseek(_fw_fd, 0, SEEK_SET); + + while (sent < fw_size) { + /* get more bytes to program */ + size_t n = fw_size - sent; + + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; + } + + count = read_with_retry(_fw_fd, file_buf, n); + + /* fill the rest with 0xff */ + if (n < PROG_MULTI_MAX) { + /* if the file can read data */ + if (count > 0) { + for (uint8_t i = count; i < PROG_MULTI_MAX; i++) { + file_buf[i] = 0xff; + } + + count = PROG_MULTI_MAX; + n = PROG_MULTI_MAX; + } + } + + if (count != (ssize_t)n) { + PX4_DEBUG("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + ret = count; + break; + } + + sent += count; + + /* send program packet */ + EscUploaderMessage program_packet = {PACKET_HEAD, sizeof(EscbusBootProgPacket), PROTO_PROG_MULTI}; + program_packet.d.program_packet.myID = esc_id; + + for (uint8_t i = 0; i < PROG_MULTI_MAX; i++) { + program_packet.d.program_packet.data[i] = file_buf[i]; + } + + send_packet(program_packet, esc_id); + + /* read and parse program feedback packet, blocking 100ms */ + ret = read_and_parse_data(100); + + if (ret != OK) { + break; + } + + /* check program is ok or fail */ + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.feedback_packet.myID != esc_id) { + PX4_DEBUG("program id don't match myID: 0x%02x,esc_id: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id); + return -EIO; + } + + if (_uploader_packet.d.feedback_packet.command != PROTO_PROG_MULTI) { + PX4_DEBUG("program bad command, myID: 0x%02x,command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("program failed: myID: 0x%02x, esc_id: 0x%02x command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("program invalid: myID: 0x%02x, esc_id: 0x%02x command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } + + } + + delete [] file_buf; + return ret; +} + +int TAP_ESC_UPLOADER::verify_crc(uint8_t esc_id, size_t fw_size_local) +{ + int ret; + uint8_t *file_buf; + ssize_t count; + uint32_t sum = 0; + uint32_t bytes_read = 0; + uint32_t crc = 0; + uint32_t fw_size_remote; + uint8_t fill_blank = 0xff; + + file_buf = new uint8_t[PROG_MULTI_MAX]; + + PX4_DEBUG("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_FW_SIZE, fw_size_remote); + + if (ret != OK) { + PX4_DEBUG("could not read firmware size"); + return ret; + } + + /* read through the firmware file again and calculate the checksum*/ + while (bytes_read < fw_size_local) { + size_t n = fw_size_local - bytes_read; + + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; + } + + count = read_with_retry(_fw_fd, file_buf, n); + + /* fill the rest with 0xff */ + if (n < PROG_MULTI_MAX) { + /* if the file can read data */ + if (count > 0) { + for (uint8_t i = count; i < PROG_MULTI_MAX; i++) { + file_buf[i] = fill_blank; + } + + count = PROG_MULTI_MAX; + n = PROG_MULTI_MAX; + } + } + + if (count != (ssize_t)n) { + PX4_DEBUG("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)bytes_read, + (int)count, + (int)errno); + ret = count; + } + + /* set the rest to 0xff */ + if (count == 0) { + break; + } + + /* stop if the file cannot be read */ + if (count < 0) { + return count; + } + + /* calculate crc32 sum */ + sum = crc32part(file_buf, PROG_MULTI_MAX, sum); + + bytes_read += count; + } + + /* fill the rest with 0xff */ + while (bytes_read < fw_size_remote) { + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); + bytes_read += sizeof(fill_blank); + } + + /* send flash crc packet */ + EscUploaderMessage flash_crc_packet = {PACKET_HEAD, sizeof(EscbusFlashCRCPacket), PROTO_GET_CRC}; + flash_crc_packet.d.flash_crc_packet.myID = esc_id; + send_packet(flash_crc_packet, esc_id); + + /* read and parse feedback CRC from tap esc,blocking 50ms */ + ret = read_and_parse_data(); + + if (ret != OK) { + return ret; + } + + /* check flash crc feedback is ok or fail */ + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.feedback_crc_packet.myID != esc_id) { + PX4_DEBUG("flash crc check id don't match myID: 0x%02x,esc_id: 0x%02x", _uploader_packet.d.feedback_packet.myID, + esc_id); + return -EIO; + } + + if (_uploader_packet.d.feedback_crc_packet.command != PROTO_GET_CRC) { + PX4_DEBUG("flash crc check bad command, myID: 0x%02x command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + crc = _uploader_packet.d.feedback_crc_packet.crc32; + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("flash crc check failed: myID: 0x%02x, esc_id: 0x%02x, command: 0x%02x", + _uploader_packet.d.feedback_packet.myID, + esc_id, _uploader_packet.d.feedback_packet.command); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("flash crc check invalid: myID: 0x%02x, esc_id: 0x%02x, command: 0x%02x", + _uploader_packet.d.feedback_packet.myID, + esc_id, _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + /* compare the CRC sum from the IO with the one calculated */ + if (sum != crc) { + PX4_DEBUG("CRC mismatch: received: %u, expected: %u", static_cast(crc), static_cast(sum)); + return -EINVAL; + } + + delete [] file_buf; + return OK; +} + +int TAP_ESC_UPLOADER::reboot(uint8_t esc_id) +{ + int ret; + + /* send reboot packet */ + EscUploaderMessage reboot_packet = {PACKET_HEAD, sizeof(EscbusRebootPacket), PROTO_REBOOT}; + reboot_packet.d.reboot_packet.myID = esc_id; + send_packet(reboot_packet, esc_id); + + /* read and parse reboot feedback packet, blocking 100ms */ + ret = read_and_parse_data(); + + if (ret != OK) { + return ret; + } + + /* check reboot feedback is ok or fail */ + if (_uploader_packet.msg_id == PROTO_OK) { + if (_uploader_packet.d.feedback_packet.myID != esc_id) { + PX4_DEBUG("reboot id don't match myID: 0x%02x,esc_id: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id); + return -EIO; + } + + if (_uploader_packet.d.feedback_packet.command != PROTO_REBOOT) { + PX4_DEBUG("reboot receive bad command, myID: 0x%02x,command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + _uploader_packet.d.feedback_packet.command); + return -EIO; + } + + } else if (_uploader_packet.msg_id == PROTO_FAILED) { + PX4_DEBUG("reboot fail, myID: 0x%02x, esc_id: 0x%02x, command: 0x%02x", _uploader_packet.d.feedback_packet.myID, esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } else if (_uploader_packet.msg_id == PROTO_INVALID) { + PX4_DEBUG("reboot invalid,myID: 0x%02x, esc_id: 0x%02x, command: 0x%02x", _uploader_packet.d.feedback_packet.myID, + esc_id, + _uploader_packet.d.feedback_packet.command); + return -EIO; + + } + + return OK; +} + +int TAP_ESC_UPLOADER::read_esc_version_from_bin(int fw_fd, uint32_t &ver) +{ + int ret = -1; + uint8_t version_buf[4]; + + /* Jump to first byte of encoded firmware version */ + ret = lseek(fw_fd, ESC_VERSION_OFFSET_ADDR, SEEK_SET); + + if (ret < 0) { + return ret; + } + + ret = read(fw_fd, version_buf, ESC_FW_VER_BYTES); + + if (ret < 0) { + return ret; + } + + /* Decode ESC firmware version (little-Endian)*/ + ver = (version_buf[3] << 24) + (version_buf[2] << 16) + (version_buf[1] << 8) + version_buf[0]; + + /* check the decoded value is invalid */ + if (!PX4_ISFINITE((double)ver) || ver == 0) { + return -EBADRQC; + } + + return OK; +} + +int TAP_ESC_UPLOADER::update_fw(const char *filenames[]) +{ + int ret = initialise_firmware_file(filenames, _fw_fd); + + if (ret < 0) { + PX4_DEBUG("initialise firmware file failed"); + return ret; + } + + uint32_t fw_binary_version = 0; + ret = read_esc_version_from_bin(_fw_fd, fw_binary_version); + + /* Catch errors if firmware version could not be read from binay */ + if (ret == -EBADRQC) { + PX4_WARN("Firmware version from binary is invalid - not updating!"); + return ret; + + } else if (ret != OK) { + PX4_WARN("Could not read firmware version from binary - not updating!"); + return ret; + } + + ret = tap_esc_common::initialise_uart(_device, _esc_fd); + + if (ret < 0) { + PX4_DEBUG("Failed to initialize UART device %s (error code %i)", _device, ret); + return ret; + } + + /* check firmware version of each ESC */ + for (uint8_t esc_id = 0; esc_id < _esc_counter; esc_id++) { + + uint32_t fw_esc_version = 0; + + /* get device firmware revision */ + ret = get_device_info(esc_id, ESCBUS_MSG_ID_REQUEST_INFO, REQUEST_INFO_DEVICE, fw_esc_version); + + /* check if ESC firmware is too old to understand command, in which case the + response runs into a timeout */ + if (ret == -ETIMEDOUT) { + PX4_INFO("ESC did not respond to version request command. Probably out of date."); + /* Check CRC of ESC firmware and compare to CRC of binary file. On + mismatch the firmware is re-upload to the ESC. This will also + reboot the ESCs after it is done */ + return checkcrc(filenames); + } + + /* check if firmware version of binary is newer than firmware currently on ESCs */ + else if (ret == OK && (fw_binary_version > fw_esc_version)) { + PX4_INFO("esc_id %d has fw version mismatch (%4.4f instead of %4.4f)", esc_id, (double)fw_esc_version * 0.01, + (double)fw_binary_version * 0.01); + PX4_INFO("At least one ESC has a version mismatch. Running checkcrc for all ESCs!"); + /* Check CRC of ESC firmware and compare to CRC of binary file. On + mismatch the firmware is re-upload to the ESC. This will also + reboot the ESCs after it is done */ + return checkcrc(filenames); + } + } + + /* ESCs did not require update */ + return CODE_ESCS_ALREADY_UPTODATE; +} diff --git a/src/drivers/tap_esc/tap_esc_uploader.h b/src/drivers/tap_esc/tap_esc_uploader.h new file mode 100644 index 0000000000..1f1a0964bf --- /dev/null +++ b/src/drivers/tap_esc/tap_esc_uploader.h @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tap_esc_uploader.h + * Firmware uploader definitions for TAP ESC. + */ + +#ifndef _TAP_ESC_UPLOADER_H +#define _TAP_ESC_UPLOADER_H + +#include +#include +#include +#include + +#define TAP_ESC_FW_SEARCH_PATHS {"/etc/extras/tap_esc.bin", "/fs/microsd/tap_esc.bin", nullptr } +#define PROTO_SUPPORT_BL_REV 5 /**< supported bootloader protocol revision */ +#define SYNC_RETRY_TIMES 5 /**< (uint8) esc sync failed allow retry times*/ +#define UPLOADER_RETRY_TIMES 2 /**< esc uploader failed allow retry times*/ +#define ESCBUS_DATA_CRC_LEN 248 /**< length of data field is 255 and plus one byte for CRC*/ +#define ESC_WAIT_BEFORE_READ 1 /**< ms, wait before reading to save read() calls*/ +#define ESC_VERSION_OFFSET_ADDR 0x200 /**< ESCs firmware version offset address in tap_esc.bin file */ +#define ESC_FW_VER_BYTES 4 /**< Number of bytes used to encode ESC firmware version */ +#define CODE_ESCS_ALREADY_UPTODATE 1 + +class TAP_ESC_UPLOADER +{ +public: + TAP_ESC_UPLOADER(const char *device, uint8_t esc_counter); + virtual ~TAP_ESC_UPLOADER(); + + /* + * Upload ESC firmware from a binary. + * @param filenames + * @return OK on success, -errno otherwise + */ + int upload(const char *filenames[]); + + /* + * Compare ESC firmware CRC with that of a binary file. + * In case of mismatch, the binary file is uploaded to the ESCs. + * @param filenames + * @return OK on success, -errno otherwise + */ + int checkcrc(const char *filenames[]); + + /* + * Query ESCs for version information (firmware, hardware, bootloader) + * and store the response in the corresponding parameters. + */ + int log_versions(); + + /* + * read version of ESC firmware from tap_esc.bin file + * @param filenames + * @param ver: get the ESC firmware version in format xx.yy * 100 + * @return OK on success, or -errno otherwise. + */ + static int read_esc_version_from_bin(int fw_fd, uint32_t &ver); + + /* + * Update ESC firmware with newer version from binary, if necessary. + * @param filenames + * @return 'CODE_ESCS_ALREADY_UPTODATE' if no update is required, 'PX4_OK' if updated, or '-errno' otherwise. + */ + int update_fw(const char *filenames[]); + + /* + * Open firmware binary file from a prioritized list of filenames + * @param filenames array of paths to try and open, must be NULL-terminated + * @param fw_fd file descriptor for firmware binary + * @return file size on success or -errno on failure + */ + static int32_t initialise_firmware_file(const char *filenames[], int &fw_fd); + +private: + +#pragma pack(push,1) + + typedef enum { + + PROTO_NOP = 0x00, + + /** + * receive tap esc feedback information + */ + PROTO_OK = 0x10, /**< INSYNC/OK - 'ok' response */ + PROTO_FAILED = 0x11, /**< INSYNC/FAILED - 'fail' response */ + PROTO_INVALID = 0x13, /**< INSYNC/INVALID - 'invalid' response for bad commands */ + PROTO_BAD_SILICON_REV = 0x14, /**< On the F4 series there is an issue with < Rev 3 silicon */ + + /** + * send to tap esc command information + */ + PROTO_GET_SYNC = 0x21, /**< NOP for re-establishing sync */ + PROTO_GET_DEVICE_INFO = 0x22, /**< get device ID bytes */ + PROTO_CHIP_ERASE = 0x23, /**< erase program area and reset program address */ + PROTO_PROG_MULTI = 0x27, /**< write bytes at program address and increment */ + PROTO_GET_CRC = 0x29, /**< compute & return a CRC */ + PROTO_GET_OTP = 0x2a, /**< read a byte from OTP at the given address */ + PROTO_GET_SN = 0x2b, /**< read a word from UDID area ( Serial) at the given address */ + PROTO_GET_CHIP = 0x2c, /**< read chip version (MCU IDCODE) */ + PROTO_SET_DELAY = 0x2d, /**< set minimum boot delay */ + PROTO_GET_CHIP_DES = 0x2e, /**< read chip version In ASCII */ + PROTO_REBOOT = 0x30, /**< boot the application */ + PROTO_MSG_ID_MAX_NUM = 0x40, /**< support protocol maximum message ID command */ + + /* argument values for PROTO_GET_DEVICE */ + PROTO_DEVICE_BL_REV = 1, /**< bootloader revision */ + PROTO_DEVICE_BOARD_ID = 2, /**< board ID Use for distinguishing ESC chip, each chip corresponds to a fixed value */ + PROTO_DEVICE_BOARD_REV = 3, /**< board revision */ + PROTO_DEVICE_FW_SIZE = 4, /**< size of flashable area */ + PROTO_DEVICE_VEC_AREA = 5, /**< contents of reserved vectors 7-10 */ + PROTO_DEVICE_FW_REV = 6, /**< firmware revision */ + + PROG_MULTI_MAX = 128, /**< protocol max is 255, must be multiple of 4 */ + + } ESCBUS_UPLOADER_MESSAGE_ID; + + /**< tap esc feedback packet,The command is the same as the command received */ + typedef struct { + uint8_t myID; + ESCBUS_UPLOADER_MESSAGE_ID command; + } EscbusBootFeedbackPacket; + + /**< the real packet definition for PROTO_GET_SYNC */ + typedef struct { + uint8_t myID; + } EscbusBootSyncPacket; + + /**< the real packet definition for PROTO_CHIP_ERASE */ + typedef struct { + uint8_t myID; + } EscbusBootErasePacket; + + /**< the real packet definition for PROTO_PROG_MULTI */ + typedef struct { + uint8_t myID; + uint8_t data[PROG_MULTI_MAX]; + } EscbusBootProgPacket; + + /**< the real packet definition for PROTO_GET_CRC */ + typedef struct { + uint8_t myID; + } EscbusFlashCRCPacket; + + /**< the real packet feedback for CRC sum */ + typedef struct { + uint8_t myID; + uint32_t crc32; /**< The offset address of verification, the length is fixed 128 bytes */ + ESCBUS_UPLOADER_MESSAGE_ID command;/**< The command is the same as the command received */ + } EscbusBootFeedbackCRCPacket; + + /**< the real packet definition for PROTO_REBOOT */ + typedef struct { + uint8_t myID; + } EscbusRebootPacket; + + /**< the real packet definition for PROTO_DEVICE */ + typedef struct { + uint8_t myID; + uint8_t deviceInfo; + } EscbusGetDevicePacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_BL_REV */ + typedef struct { + uint8_t myID; + uint32_t version; + } EscbusBootloaderRevisionPacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_BOARD_ID */ + typedef struct { + uint8_t myID; + uint32_t targetSystemId;/**< Use for distinguishing ESC chip, each chip corresponds to a fixed value(e.g. H520 targetSystemId is 0x02) */ + } EscbusHardwareIDPacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_BOARD_REV */ + typedef struct { + uint8_t myID; + uint32_t boardRev; + } EscbusHardwareRevisionPacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_VERSION */ + typedef struct { + uint8_t myID; + uint32_t FwRev; + uint32_t HwRev; + uint32_t blRev; + } EscbusDeviceInfoPacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_FW_SIZE */ + typedef struct { + uint8_t myID; + uint32_t FwSize; + } EscbusFirmwareSizePacket; + + /**< the real packet definition for deviceInfo is PROTO_DEVICE_FW_REV */ + typedef struct { + uint8_t myID; + uint32_t FwRev; + } EscbusFirmwareRevisionPacket; + + /**< the real packet definition for PROTO_INVALID,When the package to ESC is parsed fail, ESC sends the command */ + typedef struct { + uint8_t myID; + } EscbusProtocolInvalidPacket; + + /* + * ESCBUS bootloader message type + */ + typedef struct { + uint8_t head; /**< start sign for a message package */ + uint8_t len; /**< length of data field */ + uint8_t msg_id; /**< ID for this message */ + union { + EscbusBootFeedbackPacket feedback_packet; + EscbusBootSyncPacket sync_packet; + EscbusBootErasePacket erase_packet; + EscbusBootProgPacket program_packet; + EscbusFlashCRCPacket flash_crc_packet; + EscbusBootFeedbackCRCPacket feedback_crc_packet; + EscbusRebootPacket reboot_packet; + EscbusGetDevicePacket device_info_packet; + EscbusBootloaderRevisionPacket bootloader_revis_packet; + EscbusHardwareIDPacket hardware_id_packet; + EscbusHardwareRevisionPacket hardware_revis_packet; + EscbusFirmwareSizePacket firmware_size_packet; + EscbusFirmwareRevisionPacket firmware_revis_packet; + EscbusProtocolInvalidPacket protocal_invalid_packet; + EscbusDeviceInfoPacket esc_version_packet; + uint8_t data[ESCBUS_DATA_CRC_LEN]; /**< length of data field is 255 and plus one byte for CRC */ + } d; + uint8_t crc_data; + + } EscUploaderMessage; + +#pragma pack(pop) + + int _esc_fd{-1}; + int _fw_fd{-1}; + const char *_device; + uint8_t _esc_counter{0}; + + /* _device_mux_map[sel]:Asign the id's to the ESC to match the mux */ + static const uint8_t _device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; + EscUploaderMessage _uploader_packet{}; + orb_advert_t _mavlink_log_pub{nullptr}; + int upload_id(uint8_t esc_id, int32_t fw_size); + int recv_byte_with_timeout(uint8_t *c, unsigned timeout); + int read_and_parse_data(unsigned timeout = 50); + int parse_tap_esc_feedback(uint8_t decode_data, EscUploaderMessage *packetdata); + uint8_t crc8_esc(uint8_t *p, uint8_t len); + uint8_t crc_packet(EscUploaderMessage &p); + int send_packet(EscUploaderMessage &packet, int responder); + int sync(uint8_t esc_id); + int get_device_info(uint8_t esc_id, uint8_t msg_id, uint8_t msg_arg, uint32_t &val); + int get_esc_versions(uint8_t esc_id, uint32_t &fw_ver, uint32_t &hw_ver, uint32_t &bl_ver); + int erase(uint8_t esc_id); + int program(uint8_t esc_id, size_t fw_size); + int verify_crc(uint8_t esc_id, size_t fw_size_local); + int reboot(uint8_t esc_id); +}; +#endif