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

drivers/tap_esc: Advanced Technology Labs (ATL) Mantis EDU ESC support

Browse Source 
Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Daniel Agar 2021-07-13 21:04:29 -04:00
parent e900f2ea54
commit 6ab8153f32
10 changed files with 3396 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
boards/atl/mantis-edu/src/board_config.h
View File

@ -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)

49
src/drivers/tap_esc/CMakeLists.txt Normal file
View File

@ -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
)

779
src/drivers/tap_esc/TAP_ESC.cpp Normal file
View File

@ -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 &timestamp_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>", "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);
}

173
src/drivers/tap_esc/TAP_ESC.hpp Normal file
View File

@ -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 <stdint.h>
#include <px4_defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/module_params.h>
#include <errno.h>
#include <math.h> // NAN
#include <cstring>
#include <lib/mathlib/mathlib.h>
#include <lib/cdev/CDev.hpp>
#include <lib/led/led.h>
#include <lib/perf/perf_counter.h>
#include <lib/tunes/tunes.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/led_control.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#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<TAP_ESC>, 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<actuator_outputs_s> _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_status_s> _esc_feedback_pub{ORB_ID(esc_status)};
uORB::PublicationMulti<multirotor_motor_limits_s> _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<px4::params::MC_AIRMODE>) _param_mc_airmode ///< multicopter air-mode
)
};

292
src/drivers/tap_esc/drv_tap_esc.h Normal file
View File

@ -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 <board_config.h>
#include <stdint.h>
#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;

312
src/drivers/tap_esc/tap_esc_common.cpp Normal file
View File

@ -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 <fcntl.h>
#include <termios.h>
#include <systemlib/px4_macros.h> // arraySize
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#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 */

106
src/drivers/tap_esc/tap_esc_common.h Normal file
View File

@ -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 <stdint.h>
#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 */

32
src/drivers/tap_esc/tap_esc_params.c Normal file
View File

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

1356
src/drivers/tap_esc/tap_esc_uploader.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

294
src/drivers/tap_esc/tap_esc_uploader.h Normal file
View File

@ -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 <drivers/tap_esc/drv_tap_esc.h>
#include <stdint.h>
#include <stdbool.h>
#include <systemlib/mavlink_log.h>
#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