From f591988f32db9f4deb893a497d90bde6be077670 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 6 Jul 2022 18:00:50 -0400 Subject: [PATCH] drivers/actuators: modalai_esc driver Co-authored-by: Travis Bottalico Co-authored-by: akushley --- boards/modalai/fc-v1/default.px4board | 7 +- boards/modalai/fc-v2/default.px4board | 7 +- src/drivers/actuators/Kconfig | 3 + .../actuators/modalai_esc/CMakeLists.txt | 50 + src/drivers/actuators/modalai_esc/Kconfig | 5 + src/drivers/actuators/modalai_esc/crc16.c | 95 ++ src/drivers/actuators/modalai_esc/crc16.h | 65 + .../actuators/modalai_esc/modalai_esc.cpp | 1492 +++++++++++++++++ .../actuators/modalai_esc/modalai_esc.hpp | 237 +++ .../modalai_esc/modalai_esc_serial.cpp | 163 ++ .../modalai_esc/modalai_esc_serial.hpp | 61 + .../actuators/modalai_esc/parameters.c | 167 ++ .../actuators/modalai_esc/qc_esc_packet.c | 254 +++ .../actuators/modalai_esc/qc_esc_packet.h | 275 +++ .../modalai_esc/qc_esc_packet_types.h | 73 + src/lib/parameters/px4params/srcparser.py | 1 + 16 files changed, 2949 insertions(+), 6 deletions(-) create mode 100644 src/drivers/actuators/Kconfig create mode 100644 src/drivers/actuators/modalai_esc/CMakeLists.txt create mode 100644 src/drivers/actuators/modalai_esc/Kconfig create mode 100644 src/drivers/actuators/modalai_esc/crc16.c create mode 100644 src/drivers/actuators/modalai_esc/crc16.h create mode 100644 src/drivers/actuators/modalai_esc/modalai_esc.cpp create mode 100644 src/drivers/actuators/modalai_esc/modalai_esc.hpp create mode 100644 src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp create mode 100644 src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp create mode 100644 src/drivers/actuators/modalai_esc/parameters.c create mode 100644 src/drivers/actuators/modalai_esc/qc_esc_packet.c create mode 100644 src/drivers/actuators/modalai_esc/qc_esc_packet.h create mode 100644 src/drivers/actuators/modalai_esc/qc_esc_packet_types.h diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index fca6849eca..d3b8b01aa3 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_COMMON_BAROMETERS=y @@ -42,6 +43,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -50,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -65,7 +68,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -74,10 +76,9 @@ CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index 5f8fdb69f3..eebffc40c8 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_COMMON_BAROMETERS=y @@ -40,6 +41,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -48,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -63,17 +66,15 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/src/drivers/actuators/Kconfig b/src/drivers/actuators/Kconfig new file mode 100644 index 0000000000..e5badc79c8 --- /dev/null +++ b/src/drivers/actuators/Kconfig @@ -0,0 +1,3 @@ +menu "actuators" +rsource "*/Kconfig" +endmenu #actuators diff --git a/src/drivers/actuators/modalai_esc/CMakeLists.txt b/src/drivers/actuators/modalai_esc/CMakeLists.txt new file mode 100644 index 0000000000..2e1bc60a27 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2020 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__actuators__modalai_esc + MAIN modalai_esc + SRCS + crc16.c + crc16.h + + modalai_esc_serial.cpp + modalai_esc_serial.hpp + modalai_esc.cpp + modalai_esc.hpp + qc_esc_packet_types.h + qc_esc_packet.c + qc_esc_packet.h + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/actuators/modalai_esc/Kconfig b/src/drivers/actuators/modalai_esc/Kconfig new file mode 100644 index 0000000000..b5996caf64 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ACTUATORS_MODALAI_ESC + bool "modalai_esc" + default n + ---help--- + Enable support for modalai_esc diff --git a/src/drivers/actuators/modalai_esc/crc16.c b/src/drivers/actuators/modalai_esc/crc16.c new file mode 100644 index 0000000000..78a7dddaec --- /dev/null +++ b/src/drivers/actuators/modalai_esc/crc16.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include +#include "crc16.h" + +// Look-up table for fast CRC16 computations +const uint16_t crc16_table[256] = { + 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, + 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, + 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, + 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, + 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, + 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, + 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, + 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, + 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, + 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, + 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, + 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, + 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, + 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, + 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, + 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, + 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, + 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, + 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, + 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, + 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, + 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, + 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, + 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, + 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, + 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, + 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, + 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, + 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, + 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, + 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, + 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, +}; + +uint16_t crc16_init() +{ + return 0xFFFF; +} + +uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte) +{ + uint8_t lut = (prev_crc ^ new_byte) & 0xFF; + return (prev_crc >> 8) ^ crc16_table[lut]; +} + +uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length) +{ + uint16_t crc = prev_crc; + + while (input_length--) { + crc = crc16_byte(crc, *input_buffer++); + } + + return crc; +} diff --git a/src/drivers/actuators/modalai_esc/crc16.h b/src/drivers/actuators/modalai_esc/crc16.h new file mode 100644 index 0000000000..c1a308100d --- /dev/null +++ b/src/drivers/actuators/modalai_esc/crc16.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for crc16 computations using polynomial 0x8005 + */ + +#ifndef CRC16_H_ +#define CRC16_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +// Returns the seed of crc output, which should be used when computing crc16 of a byte sequence +uint16_t crc16_init(void); + +// Process one byte by providing crc16 from previous step and new byte to consume. +// Output is the new crc16 value +uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte); + +// Process an array of bytes by providing crc16 from previous step (or seed), array of bytes and its length +// Output is the new crc16 value +uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length); + +#ifdef __cplusplus +} +#endif + +#endif //CRC16_H_ diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.cpp b/src/drivers/actuators/modalai_esc/modalai_esc.cpp new file mode 100644 index 0000000000..dba637dda1 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/modalai_esc.cpp @@ -0,0 +1,1492 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + +#include + +#include "modalai_esc.hpp" +#include "modalai_esc_serial.hpp" + +#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc" +#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1" +#define MODALAI_ESC_VOXL_PORT "/dev/ttyS4" + +//TODO: make this a param!!! +#define MODALAI_PUBLISH_ESC_STATUS 1 + +const char *_device; + +ModalaiEsc::ModalaiEsc() : + CDev(MODALAI_ESC_DEVICE_PATH), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) +{ + _device = MODALAI_ESC_DEFAULT_PORT; + + _mixing_output.setAllFailsafeValues(0); + _mixing_output.setAllDisarmedValues(0); + + _esc_status.timestamp = hrt_absolute_time(); + _esc_status.counter = 0; + _esc_status.esc_count = MODALAI_ESC_OUTPUT_CHANNELS; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + + for (unsigned i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + _esc_status.esc[i].timestamp = 0; + _esc_status.esc[i].esc_address = 0; + _esc_status.esc[i].esc_rpm = 0; + _esc_status.esc[i].esc_state = 0; + //_esc_status.esc[i].esc_cmdcount = 0; + _esc_status.esc[i].esc_voltage = 0; + _esc_status.esc[i].esc_current = 0; + _esc_status.esc[i].esc_temperature = 0; + _esc_status.esc[i].esc_errorcount = 0; + _esc_status.esc[i].failures = 0; + } + + qc_esc_packet_init(&_fb_packet); + qc_esc_packet_init(&_uart_bridge_packet); + + _fb_idx = 0; +} + +ModalaiEsc::~ModalaiEsc() +{ + _outputs_on = false; + + if (_uart_port) { + _uart_port->uart_close(); + _uart_port = nullptr; + } + + if (_uart_port_bridge) { + _uart_port_bridge->uart_close(); + _uart_port_bridge = nullptr; + } + + /* clean up the alternate device node */ + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); + + perf_free(_cycle_perf); + perf_free(_output_update_perf); +} + +int ModalaiEsc::init() +{ + /* do regular cdev init */ + int ret = CDev::init(); + + if (ret != OK) { + return ret; + } + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + _class_instance = register_class_devname(MODALAI_ESC_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* lets not be too verbose */ + } else if (_class_instance < 0) { + PX4_ERR("FAILED registering class device"); + } + + _mixing_output.setDriverInstance(_class_instance); + + /* Getting initial parameter values */ + ret = update_params(); + + if (ret != OK) { + return ret; + } + + _uart_port = new ModalaiEscSerial(); + _uart_port_bridge = new ModalaiEscSerial(); + + //get_instance()->ScheduleOnInterval(10000); //100hz + + ScheduleNow(); + + return 0; +} + +int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map) +{ + int ret = PX4_OK; + + param_get(param_find("UART_ESC_CONFIG"), ¶ms->config); + param_get(param_find("UART_ESC_MODE"), ¶ms->mode); + param_get(param_find("UART_ESC_DEAD1"), ¶ms->dead_zone_1); + param_get(param_find("UART_ESC_DEAD2"), ¶ms->dead_zone_2); + param_get(param_find("UART_ESC_BAUD"), ¶ms->baud_rate); + param_get(param_find("UART_ESC_MOTOR1"), ¶ms->motor_map[0]); + param_get(param_find("UART_ESC_MOTOR2"), ¶ms->motor_map[1]); + param_get(param_find("UART_ESC_MOTOR3"), ¶ms->motor_map[2]); + param_get(param_find("UART_ESC_MOTOR4"), ¶ms->motor_map[3]); + param_get(param_find("UART_ESC_RPM_MIN"), ¶ms->rpm_min); + param_get(param_find("UART_ESC_RPM_MAX"), ¶ms->rpm_max); + + if (params->rpm_min >= params->rpm_max) { + PX4_ERR("Invalid parameter UART_ESC_RPM_MIN. Please verify parameters."); + params->rpm_min = 0; + ret = PX4_ERROR; + } + + // Example, PX4 Motor 1 + // X = don't activate + // [setpoint.x] + // [1.0] + // ' | + // ' | + // ' | + // ' | (ACTIVATE) + // ' | + // DEADZONE_1 + - + - - - + + // X X|X X X X X' + // DEADZONE_2 - X X+X X X X X+ + // X X|X X X X X' + // [0.0]-+---+---+-----+---------------- [1.0] [setpoint.y] + // / X X|X X X X X' (ACTIVATE) + // / X X|X X X X X' + // / [0.0] + - - - - - + // -(DEADZONE_2) \ DEADZONE_2 + // + + if ((params->dead_zone_1 < MODALAI_ESC_MODE_DEAD_ZONE_MIN) || (params->dead_zone_2 < MODALAI_ESC_MODE_DEAD_ZONE_MIN) || + (params->dead_zone_1 >= MODALAI_ESC_MODE_DEAD_ZONE_MAX) || (params->dead_zone_2 >= MODALAI_ESC_MODE_DEAD_ZONE_MAX) || + (params->dead_zone_2 >= params->dead_zone_1)) { + PX4_ERR("Invalid parameter UART_ESC_DEAD1 or UART_ESC_DEAD2. Please verify parameters."); + params->dead_zone_1 = MODALAI_ESC_MODE_DEAD_ZONE_1; + params->dead_zone_2 = MODALAI_ESC_MODE_DEAD_ZONE_2; + ret = PX4_ERROR; + } + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + if (params->motor_map[i] == MODALAI_ESC_OUTPUT_DISABLED || + params->motor_map[i] < -(MODALAI_ESC_OUTPUT_CHANNELS) || + params->motor_map[i] > MODALAI_ESC_OUTPUT_CHANNELS) { + PX4_ERR("Invalid parameter UART_ESC_MOTORX. Please verify parameters."); + params->motor_map[i] = 0; + ret = PX4_ERROR; + } + + /* Can map -4 to 4, 0 being disabled. Negative represents reverse direction */ + map[i].number = abs(params->motor_map[i]); + map[i].direction = (params->motor_map[i] > 0) ? 1 : -1; + } + + return ret; +} + +int ModalaiEsc::task_spawn(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _device = argv[myoptind]; + break; + + default: + break; + } + } + + ModalaiEsc *instance = new ModalaiEsc(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ModalaiEsc::flushUartRx() +{ + while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} + + return 0; +} + +int ModalaiEsc::readResponse(Command *out_cmd) +{ + px4_usleep(_current_cmd.resp_delay_us); + + int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); + + if (res > 0) { + //PX4_INFO("read %i bytes",res); + if (parseResponse(_read_buf, res, out_cmd->print_feedback) < 0) { + //PX4_ERR("Error parsing response"); + return -1; + } + + } else { + //PX4_ERR("Read error: %i", res); + return -1; + } + + //_current_cmd.response = false; + + return 0; +} + +int ModalaiEsc::parseResponse(uint8_t *buf, uint8_t len, bool print_feedback) +{ + hrt_abstime tnow = hrt_absolute_time(); + + for (int i = 0; i < len; i++) { + int16_t ret = qc_esc_packet_process_char(buf[i], &_fb_packet); + + if (ret > 0) { + //PX4_INFO("got packet of length %i",ret); + uint8_t packet_type = qc_esc_packet_get_type(&_fb_packet); + uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet); + + if (packet_type == ESC_PACKET_TYPE_FB_RESPONSE && packet_size == sizeof(QC_ESC_FB_RESPONSE_V2)) { + //PX4_INFO("Got feedback V2 packet!"); + QC_ESC_FB_RESPONSE_V2 fb; + memcpy(&fb, _fb_packet.buffer, packet_size); + + uint32_t id = (fb.id_state & 0xF0) >> 4; //ID of the ESC based on hardware address + + if (id < MODALAI_ESC_OUTPUT_CHANNELS) { + + int motor_idx = _output_map[id].number - 1; // mapped motor id.. user defined mapping is 1-4, array is 0-3 + + if (print_feedback) { + uint32_t rpm = fb.rpm; + uint32_t power = fb.power; + uint32_t voltage = fb.voltage; + int32_t current = fb.current * 8; + int32_t temperature = fb.temperature / 100; + PX4_INFO("[%lld] ID_RAW=%" PRIu32 " ID=%d, RPM=%5" PRIu32 ", PWR=%3" PRIu32 "%%, V=%5" PRIu32 "mV, I=%+5" PRIi32 + "mA, T=%+3" PRIi32 "C", + tnow, id, motor_idx + 1, rpm, power, voltage, current, temperature); + } + + _esc_chans[id].rate_meas = fb.rpm; + _esc_chans[id].power_applied = fb.power; + _esc_chans[id].state = fb.id_state & 0x0F; + _esc_chans[id].cmd_counter = fb.cmd_counter; + _esc_chans[id].voltage = fb.voltage * 0.001; + _esc_chans[id].current = fb.current * 0.008; + _esc_chans[id].temperature = fb.temperature * 0.01; + _esc_chans[id].feedback_time = tnow; + + // also update our internal report for logging + _esc_status.esc[id].esc_address = motor_idx + 1; //remapped motor ID + _esc_status.esc[id].timestamp = tnow; + _esc_status.esc[id].esc_rpm = fb.rpm; + //_esc_status.esc[id].esc_power = fb.power; + _esc_status.esc[id].esc_state = fb.id_state & 0x0F; + //_esc_status.esc[id].esc_cmdcount = fb.cmd_counter; + _esc_status.esc[id].esc_voltage = _esc_chans[id].voltage; + _esc_status.esc[id].esc_current = _esc_chans[id].current; + _esc_status.esc[id].failures = 0; //not implemented + + // use PX4 motor index here (already brough down to 0-3 above), so reporting of ESC online maps to PX4 motors + _esc_status.esc_online_flags |= (1 << motor_idx); + + // state == 0 is stopped, but in turtle mode idle is OK so consider armed + if (_esc_chans[id].state > 0 || _turtle_mode_en) { + _esc_status.esc_armed_flags |= (1 << motor_idx); + + } else { + _esc_status.esc_armed_flags &= ~(1 << motor_idx); + } + + int32_t t = fb.temperature / 100; //divide by 100 to get deg C and cap for int8 + + if (t < -127) { t = -127; } + + if (t > +127) { t = +127; } + + _esc_status.esc[id].esc_temperature = t; + + _esc_status.timestamp = _esc_status.esc[id].timestamp; + _esc_status.counter++; + + //print ESC status just for debugging + /* + PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, CNT %d, FAIL %d", + _esc_status.esc[id].timestamp, id, _esc_status.esc[id].esc_address, + _esc_status.esc[id].esc_state, _esc_status.esc[id].esc_rpm, _esc_status.esc[id].esc_power, + (double)_esc_status.esc[id].esc_voltage, (double)_esc_status.esc[id].esc_current, _esc_status.esc[id].esc_temperature, + _esc_status.esc[id].esc_cmdcount, _esc_status.esc[id].failures); + */ + } + } + + else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) { + QC_ESC_VERSION_INFO ver; + memcpy(&ver, _fb_packet.buffer, packet_size); + PX4_INFO("ESC ID: %i", ver.id); + PX4_INFO("HW Version: %i", ver.hw_version); + PX4_INFO("SW Version: %i", ver.sw_version); + PX4_INFO("Unique ID: %" PRIu32, ver.unique_id); + + } else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) { + QC_ESC_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _fb_packet.buffer, packet_size); + PX4_INFO("\tESC ID : %i", ver.id); + PX4_INFO("\tBoard : %i", ver.hw_version); + PX4_INFO("\tSW Version : %i", ver.sw_version); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + } + + } else { //parser error + /* + switch (ret) + { + case ESC_ERROR_BAD_CHECKSUM: PX4_INFO("BAD ESC packet checksum"); break; + case ESC_ERROR_BAD_LENGTH: PX4_INFO("BAD ESC packet length"); break; + } + */ + } + } + + /* + if (len < 4 || buf[0] != ESC_PACKET_HEADER) { + return -1; + } + + switch (buf[2]) { + case ESC_PACKET_TYPE_VERSION_RESPONSE: + if (len != sizeof(QC_ESC_VERSION_INFO)) { + return -1; + + } else { + QC_ESC_VERSION_INFO ver; + memcpy(&ver, buf, len); + PX4_INFO("ESC ID: %i", ver.id); + PX4_INFO("HW Version: %i", ver.hw_version); + PX4_INFO("SW Version: %i", ver.sw_version); + PX4_INFO("Unique ID: %i", ver.unique_id); + } + + break; + + case ESC_PACKET_TYPE_FB_RESPONSE: + if (len != sizeof(QC_ESC_FB_RESPONSE)) { + return -1; + + } else { + QC_ESC_FB_RESPONSE fb; + memcpy(&fb, buf, len); + uint8_t id = (fb.state & 0xF0) >> 4; + + if (id < MODALAI_ESC_OUTPUT_CHANNELS) { + _esc_chans[id].rate_meas = fb.rpm; + _esc_chans[id].state = fb.state & 0x0F; + _esc_chans[id].cmd_counter = fb.cmd_counter; + _esc_chans[id].voltage = 9.0 + fb.voltage / 34.0; + } + } + + break; + + default: + return -1; + } + */ + + return 0; +} + +int ModalaiEsc::checkForEscTimeout() +{ + hrt_abstime tnow = hrt_absolute_time(); + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + // PX4 motor indexed user defined mapping is 1-4, we want to use in bitmask (0-3) + uint8_t motor_idx = _output_map[i].number - 1; + + if (motor_idx < MODALAI_ESC_OUTPUT_CHANNELS) { + // we are using PX4 motor index in the bitmask + if (_esc_status.esc_online_flags & (1 << motor_idx)) { + // using index i here for esc_chans enumeration stored in ESC ID order + if ((tnow - _esc_chans[i].feedback_time) > MODALAI_ESC_DISCONNECT_TIMEOUT_US) { + // stale data, assume offline and clear armed + _esc_status.esc_online_flags &= ~(1 << motor_idx); + _esc_status.esc_armed_flags &= ~(1 << motor_idx); + } + } + } + } + + return 0; + +} + +int ModalaiEsc::sendCommandThreadSafe(Command *cmd) +{ + cmd->id = _cmd_id++; + _pending_cmd.store(cmd); + + /* wait until main thread processed it */ + while (_pending_cmd.load()) { + px4_usleep(1000); + } + + return 0; +} + + + +int ModalaiEsc::custom_command(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + Command cmd; + uint8_t esc_id = 255; + uint8_t period = 0; + uint8_t duration = 0; + uint8_t power = 0; + uint16_t led_mask = 0; + int16_t rate = 0; + + uint32_t repeat_count = 100; + uint32_t repeat_delay_us = 10000; + + if (argc < 3) { + return print_usage("unknown command"); + } + + const char *verb = argv[argc - 1]; + + /* start the FMU if not running */ + if (!strcmp(verb, "start")) { + if (!is_running()) { + return ModalaiEsc::task_spawn(argc, argv); + } + } + + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + + } + + while ((ch = px4_getopt(argc, argv, "i:p:d:v:l:n:r:t:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'i': + esc_id = atoi(myoptarg); + break; + + case 'p': + period = atoi(myoptarg); + break; + + case 'd': + duration = atoi(myoptarg); + break; + + case 'v': + power = atoi(myoptarg); + break; + + case 'l': + led_mask = atoi(myoptarg); + break; + + case 'n': + repeat_count = atoi(myoptarg); + + if (repeat_count < 1) { + print_usage("bad repeat_count"); + return 0; + } + + break; + + case 't': + repeat_delay_us = atoi(myoptarg); + + if (repeat_delay_us < 1) { + print_usage("bad repeat delay"); + return 0; + } + + break; + + case 'r': + rate = atoi(myoptarg); + break; + + default: + print_usage("Unknown command"); + return 0; + } + } + + if (!strcmp(verb, "reset")) { + if (esc_id < 4) { + PX4_INFO("Reset ESC: %i", esc_id); + cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = false; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC ID, use 0-3"); + return 0; + } + + } else if (!strcmp(verb, "version")) { + if (esc_id < 4) { + PX4_INFO("Request version for ESC: %i", esc_id); + cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = true; + cmd.resp_delay_us = 2000; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC ID, use 0-3"); + return 0; + } + + } else if (!strcmp(verb, "version-ext")) { + if (esc_id < 4) { + PX4_INFO("Request version for ESC: %i", esc_id); + cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = true; + cmd.resp_delay_us = 5000; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC ID, use 0-3"); + return 0; + } + + } else if (!strcmp(verb, "tone")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request tone for ESC mask: %i", esc_id); + cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = false; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "led")) { + if (led_mask <= 0x0FFF) { + get_instance()->_led_rsc.test = true; + get_instance()->_led_rsc.breath_en = false; + PX4_INFO("Request LED control for ESCs with mask: %i", led_mask); + + get_instance()->_esc_chans[0].led = (led_mask & 0x0007); + get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; + get_instance()->_esc_chans[2].led = (led_mask & 0x01C0) >> 6; + get_instance()->_esc_chans[3].led = (led_mask & 0x0E00) >> 9; + return 0; + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "rpm")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request RPM for ESC bit mask: %i - RPM: %i", esc_id, rate); + int16_t rate_req[MODALAI_ESC_OUTPUT_CHANNELS]; + int16_t outputs[MODALAI_ESC_OUTPUT_CHANNELS]; + outputs[0] = (esc_id & 1) ? rate : 0; + outputs[1] = (esc_id & 2) ? rate : 0; + outputs[2] = (esc_id & 4) ? rate : 0; + outputs[3] = (esc_id & 8) ? rate : 0; + + //the motor mapping is.. if I want to spin Motor 1 (1-4) then i need to provide non-zero rpm for motor map[m-1] + + uart_esc_params_t params; + ch_assign_t map[MODALAI_ESC_OUTPUT_CHANNELS]; + get_instance()->load_params(¶ms, (ch_assign_t *)&map); + + uint8_t id_fb_raw = 0; + uint8_t id_fb = 0; + + if (esc_id & 1) { id_fb_raw = 0; } + + else if (esc_id & 2) { id_fb_raw = 1; } + + else if (esc_id & 4) { id_fb_raw = 2; } + + else if (esc_id & 8) { id_fb_raw = 3; } + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3 + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + rate_req[i] = outputs[motor_idx] * map[i].direction; + } + + if (motor_idx == id_fb_raw) { + id_fb = i; + } + } + + cmd.len = qc_esc_create_rpm_packet4_fb(rate_req[0], + rate_req[1], + rate_req[2], + rate_req[3], + 0, + 0, + 0, + 0, + id_fb, + cmd.buf, + sizeof(cmd.buf)); + + cmd.response = true; + cmd.repeats = repeat_count; + cmd.resp_delay_us = 0; + cmd.repeat_delay_us = repeat_delay_us; + cmd.print_feedback = true; + + PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number); + PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb); + PX4_INFO("Sending UART ESC RPM command %i", rate); + + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "pwm")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request PWM for ESC mask: %i - PWM: %i", esc_id, rate); + int16_t rate_req[MODALAI_ESC_OUTPUT_CHANNELS]; + int16_t outputs[MODALAI_ESC_OUTPUT_CHANNELS]; + outputs[0] = (esc_id & 1) ? rate : 0; + outputs[1] = (esc_id & 2) ? rate : 0; + outputs[2] = (esc_id & 4) ? rate : 0; + outputs[3] = (esc_id & 8) ? rate : 0; + + uart_esc_params_t params; + ch_assign_t map[MODALAI_ESC_OUTPUT_CHANNELS]; + get_instance()->load_params(¶ms, (ch_assign_t *)&map); + + uint8_t id_fb_raw = 0; + uint8_t id_fb = 0; + + if (esc_id & 1) { id_fb_raw = 0; } + + else if (esc_id & 2) { id_fb_raw = 1; } + + else if (esc_id & 4) { id_fb_raw = 2; } + + else if (esc_id & 8) { id_fb_raw = 3; } + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + int motor_idx = map[i].number - 1; // user defined mapping is 1-4, array is 0-3 + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + rate_req[i] = outputs[motor_idx] * map[i].direction; + } + + if (motor_idx == id_fb_raw) { + id_fb = i; + } + } + + cmd.len = qc_esc_create_pwm_packet4_fb(rate_req[0], + rate_req[1], + rate_req[2], + rate_req[3], + 0, + 0, + 0, + 0, + id_fb, /* ESC ID .. need to fix for correct ID.. but what about multiple ESCs in bit mask.. */ + cmd.buf, + sizeof(cmd.buf)); + + cmd.response = true; + cmd.repeats = repeat_count; + cmd.resp_delay_us = 0; + cmd.repeat_delay_us = repeat_delay_us; + cmd.print_feedback = true; + + PX4_INFO("ESC map: %d %d %d %d", map[0].number, map[1].number, map[2].number, map[3].number); + PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb); + PX4_INFO("Sending UART ESC power command %i", rate); + + + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + } + + return print_usage("unknown command"); +} + +int ModalaiEsc::update_params() +{ + int ret = PX4_ERROR; + + updateParams(); + ret = load_params(&_parameters, (ch_assign_t *)&_output_map); + + if (ret == PX4_OK) { + _mixing_output.setAllMinValues(_parameters.rpm_min); + _mixing_output.setAllMaxValues(_parameters.rpm_max); + _rpm_fullscale = _parameters.rpm_max - _parameters.rpm_min; + } + + return ret; +} + + +int ModalaiEsc::ioctl(file *filp, int cmd, unsigned long arg) +{ + SmartLock lock_guard(_lock); + + int ret = OK; + + PX4_DEBUG("modalai_esc ioctl cmd: %d, arg: %ld", cmd, arg); + + switch (cmd) { + case PWM_SERVO_ARM: + PX4_INFO("PWM_SERVO_ARM"); + break; + + case PWM_SERVO_DISARM: + PX4_INFO("PWM_SERVO_DISARM"); + break; + + case MIXERIOCRESET: + _mixing_output.resetMixer(); + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixer(buf, buflen); + } + break; + + default: + ret = -ENOTTY; + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + + return ret; +} + +/* OutputModuleInterface */ +void ModalaiEsc::mixerChanged() +{ + /* + * This driver is only supporting 4 channel ESC + */ +} + + +void ModalaiEsc::updateLeds(vehicle_control_mode_s mode, led_control_s control) +{ + int i = 0; + uint8_t led_mask = _led_rsc.led_mask; + + if (_led_rsc.test) { + return; + } + + /* + * TODO - this is just a simple approach to get started. + * + */ + if (mode.timestamp != _led_rsc.mode.timestamp) { + _led_rsc.mode = mode; + } + + if (control.timestamp != _led_rsc.control.timestamp) { + _led_rsc.control = control; + + switch (_led_rsc.control.color) { + case led_control_s::COLOR_RED: + led_mask = QC_ESC_LED_RED_ON; + break; + + case led_control_s::COLOR_GREEN: + led_mask = QC_ESC_LED_GREEN_ON; + break; + + case led_control_s::COLOR_BLUE: + led_mask = QC_ESC_LED_BLUE_ON; + break; + + case led_control_s::COLOR_WHITE: + led_mask = QC_ESC_LED_RED_ON | QC_ESC_LED_GREEN_ON | QC_ESC_LED_BLUE_ON; + break; + + case led_control_s::COLOR_OFF: + led_mask = 0; + break; + } + + _led_rsc.breath_en = false; + + switch (_led_rsc.control.mode) { + case led_control_s::MODE_OFF: + break; + + case led_control_s::MODE_ON: + break; + + case led_control_s::MODE_DISABLED: + break; + + case led_control_s::MODE_BLINK_SLOW: + break; + + case led_control_s::MODE_BLINK_NORMAL: + break; + + case led_control_s::MODE_BLINK_FAST: + break; + + case led_control_s::MODE_BREATHE: + _led_rsc.breath_en = true; + _led_rsc.breath_counter = 0; + break; + + case led_control_s::MODE_FLASH: + break; + + default: + break; + } + + _led_rsc.led_mask = led_mask; + } + + if (_led_rsc.mode.flag_armed) { + led_mask = QC_ESC_LED_BLUE_ON; + + if (_led_rsc.mode.flag_control_position_enabled) { + led_mask = QC_ESC_LED_GREEN_ON; + + } else if (_led_rsc.mode.flag_control_offboard_enabled) { + led_mask = QC_ESC_LED_RED_ON; + } + + _led_rsc.led_mask = led_mask; + } + + if (_led_rsc.breath_en) { + /* 8 bit counter for a decent blink visual effect for + * 'breathing' use case + */ + if ((_led_rsc.breath_counter += 8) < 128) { + led_mask = 0; + } + } + + for (i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + _esc_chans[i].led = led_mask; + } +} + +/* OutputModuleInterface */ +bool ModalaiEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + if (num_outputs != MODALAI_ESC_OUTPUT_CHANNELS) { + return false; + } + + uint8_t motor_idx; + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + if (!_outputs_on || stop_motors) { + _esc_chans[i].rate_req = 0; + + } else { + motor_idx = _output_map[i].number; + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + /* user defined mapping is 1-4, array is 0-3 */ + motor_idx--; + + if (!_turtle_mode_en) { + _esc_chans[i].rate_req = outputs[motor_idx] * _output_map[i].direction; + + } else { + /* we may have rolled back into a dead zone by now, clear out */ + _esc_chans[i].rate_req = 0; + + float setpoint = 0.0f; + bool use_setpoint = false; + + /* At this point, we are switching on what PX4 motor we want to talk to */ + switch (_output_map[i].number) { + /* + * An ASCII graphic of this dead zone logic is above in load_params + */ + + /* PX4 motor 1 - front right */ + case 1: + + /* Pitch and roll */ + if (_manual_control_setpoint.x > _parameters.dead_zone_1) { + if (_manual_control_setpoint.y > -(_parameters.dead_zone_2)) { + setpoint = _manual_control_setpoint.x; + use_setpoint = true; + //PX4_ERR("motor1"); + } + + } else if (_manual_control_setpoint.y > _parameters.dead_zone_1) { + if (_manual_control_setpoint.x > -(_parameters.dead_zone_2)) { + setpoint = _manual_control_setpoint.y; + use_setpoint = true; + //PX4_ERR("motor1"); + } + } + + /* Yaw */ + if (_manual_control_setpoint.r < -(_parameters.dead_zone_1)) { + setpoint = fabs(_manual_control_setpoint.r); + use_setpoint = true; + //PX4_ERR("motor1"); + } + + break; + + /* PX4 motor 3 - front left */ + case 3: + + /* Pitch and roll */ + if (_manual_control_setpoint.x > _parameters.dead_zone_1) { + if (_manual_control_setpoint.y < _parameters.dead_zone_2) { + setpoint = _manual_control_setpoint.x; + use_setpoint = true; + //PX4_ERR("motor3"); + } + + } else if (_manual_control_setpoint.y < -(_parameters.dead_zone_1)) { + if (_manual_control_setpoint.x > -(_parameters.dead_zone_2)) { + setpoint = fabs(_manual_control_setpoint.y); + use_setpoint = true; + //PX4_ERR("motor3"); + } + } + + /* Yaw */ + if (_manual_control_setpoint.r > _parameters.dead_zone_1) { + setpoint = _manual_control_setpoint.r; + use_setpoint = true; + //PX4_ERR("motor3"); + } + + break; + + /* PX4 motor 2 - rear left */ + case 2: + + /* Pitch and roll */ + if (_manual_control_setpoint.x < -(_parameters.dead_zone_1)) { + if (_manual_control_setpoint.y < _parameters.dead_zone_2) { + setpoint = fabs(_manual_control_setpoint.x); + use_setpoint = true; + //PX4_ERR("motor2"); + } + + } else if (_manual_control_setpoint.y < -(_parameters.dead_zone_1)) { + if (_manual_control_setpoint.x < _parameters.dead_zone_2) { + setpoint = fabs(_manual_control_setpoint.y); + use_setpoint = true; + //PX4_ERR("motor2"); + } + } + + /* Yaw */ + if (_manual_control_setpoint.r < -(_parameters.dead_zone_1)) { + setpoint = fabs(_manual_control_setpoint.r); + use_setpoint = true; + //PX4_ERR("motor2"); + } + + break; + + /* PX4 motor 4- rear right */ + case 4: + + /* Pitch and roll */ + if (_manual_control_setpoint.x < -_parameters.dead_zone_1) { + if (_manual_control_setpoint.y > -_parameters.dead_zone_2) { + setpoint = fabs(_manual_control_setpoint.x); + use_setpoint = true; + //PX4_ERR("motor4"); + } + } + + if (_manual_control_setpoint.y > _parameters.dead_zone_1) { + if (_manual_control_setpoint.x < _parameters.dead_zone_2) { + setpoint = _manual_control_setpoint.y; + use_setpoint = true; + //PX4_ERR("motor4"); + } + } + + /* Yaw */ + if (_manual_control_setpoint.r > _parameters.dead_zone_1) { + setpoint = _manual_control_setpoint.r; + use_setpoint = true; + //PX4_ERR("motor4"); + } + + break; + } + + // set rate + float rate = 0.0f; + + if (use_setpoint) { + rate = (float)_parameters.rpm_min + ((float)_rpm_fullscale * setpoint); + rate = (-1.0f) * rate * (float)_output_map[i].direction; + } + + _esc_chans[i].rate_req = (int16_t)rate; + } + } + } + } + + Command cmd; + cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, + _esc_chans[1].rate_req, + _esc_chans[2].rate_req, + _esc_chans[3].rate_req, + _esc_chans[0].led, + _esc_chans[1].led, + _esc_chans[2].led, + _esc_chans[3].led, + _fb_idx, + cmd.buf, + sizeof(cmd.buf)); + + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet"); + return false; + } + + // round robin + _fb_idx = (_fb_idx + 1) % MODALAI_ESC_OUTPUT_CHANNELS; + + + /* + * Here we parse the feedback response. Rarely the packet is mangled + * but this means we simply miss a feedback response and will come back + * around in roughly 8ms for another... so don't freak out and keep on + * trucking I say + */ + int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); + + if (res > 0) { + parseResponse(_read_buf, res, false); + } + + /* handle loss of comms / disconnect */ + checkForEscTimeout(); + + // publish the actual command that we sent and the feedback received + if (MODALAI_PUBLISH_ESC_STATUS) { + // actuator_outputs_s actuator_outputs{}; + // actuator_outputs.noutputs = num_outputs; + + // for (size_t i = 0; i < num_outputs; ++i) { + // actuator_outputs.output[i] = _esc_chans[i].rate_req; + // } + + // actuator_outputs.timestamp = hrt_absolute_time(); + + // _outputs_debug_pub.publish(actuator_outputs); + + _esc_status_pub.publish(_esc_status); + } + + perf_count(_output_update_perf); + + return true; +} + + +void ModalaiEsc::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + + exit_and_cleanup(); + return; + } + + SmartLock lock_guard(_lock); + + perf_begin(_cycle_perf); + + /* Open serial port in this thread */ + if (!_uart_port->is_open()) { + if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) { + PX4_INFO("Opened UART ESC device"); + + } else { + PX4_ERR("Failed openening device"); + return; + } + } + + /* + for (int ii=0; ii<9; ii++) + { + const char * test_str = "Hello World!"; + _uart_port_bridge->uart_write((char*)test_str,12); + px4_usleep(10000); + } + */ + /* + uint8_t echo_buf[16]; + int bytes_read = _uart_port_bridge->uart_read(echo_buf,sizeof(echo_buf)); + if (bytes_read > 0) + _uart_port_bridge->uart_write(echo_buf,bytes_read); + */ + + + _mixing_output.update(); + + /* update output status if armed */ + _outputs_on = _mixing_output.armed().armed; + + /* check for parameter updates */ + if (!_outputs_on && _parameter_update_sub.updated()) { + /* clear update */ + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + /* update parameters from storage */ + update_params(); + } + + vehicle_control_mode_s vehicle_control_mode{}; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&vehicle_control_mode); + updateLeds(vehicle_control_mode, _led_rsc.control); + } + + led_control_s led_control{}; + + if (_led_update_sub.updated()) { + _led_update_sub.copy(&led_control); + updateLeds(_led_rsc.mode, led_control); + } + + /* breathing requires continuous updates */ + if (_led_rsc.breath_en) { + updateLeds(_led_rsc.mode, _led_rsc.control); + } + + if (_parameters.mode > 0) { + /* if turtle mode enabled, we go straight to the sticks, no mix */ + if (_manual_control_setpoint_sub.updated()) { + + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + + if (!_outputs_on) { + + float setpoint = MODALAI_ESC_MODE_DISABLED_SETPOINT; + + if (_parameters.mode == MODALAI_ESC_MODE_TURTLE_AUX1) { + setpoint = _manual_control_setpoint.aux1; + + } else if (_parameters.mode == MODALAI_ESC_MODE_TURTLE_AUX2) { + setpoint = _manual_control_setpoint.aux2; + } + + if (setpoint > MODALAI_ESC_MODE_THRESHOLD) { + _turtle_mode_en = true; + //PX4_ERR("turtle mode enabled\n"); + + } else { + _turtle_mode_en = false; + //PX4_ERR("turtle mode disabled\n"); + } + } + } + + if (_parameters.mode == MODALAI_ESC_MODE_UART_BRIDGE) { + if (!_uart_port_bridge->is_open()) { + if (_uart_port_bridge->uart_open(MODALAI_ESC_VOXL_PORT, 230400) == PX4_OK) { + PX4_INFO("Opened UART ESC Bridge device"); + + } else { + PX4_ERR("Failed openening UART ESC Bridge device"); + return; + } + } + + //uart passthrough test code + //run 9 times because i just don't know how to change update rate of the module from 10hz to 100hz.. + for (int ii = 0; ii < 9; ii++) { + uint8_t uart_buf[128]; + int bytes_read = _uart_port_bridge->uart_read(uart_buf, sizeof(uart_buf)); + + if (bytes_read > 0) { + _uart_port->uart_write(uart_buf, bytes_read); + + for (int i = 0; i < bytes_read; i++) { + int16_t ret = qc_esc_packet_process_char(uart_buf[i], &_uart_bridge_packet); + + if (ret > 0) { + //PX4_INFO("got packet of length %i",ret); + uint8_t packet_type = qc_esc_packet_get_type(&_uart_bridge_packet); + + //uint8_t packet_size = qc_esc_packet_get_size(&_uart_bridge_packet); + //if we received a command for ESC to reset, most likely firmware update is coming, switch to bootloader baud rate + if (packet_type == ESC_PACKET_TYPE_RESET_CMD) { + int bootloader_baud_rate = 230400; + + if (_uart_port->uart_get_baud() != bootloader_baud_rate) { + px4_usleep(5000); + _uart_port->uart_set_baud(bootloader_baud_rate); + } + + } else { + if (_uart_port->uart_get_baud() != _parameters.baud_rate) { + px4_usleep(5000); + _uart_port->uart_set_baud(_parameters.baud_rate); //restore normal baud rate + } + } + } + } + } + + bytes_read = _uart_port->uart_read(uart_buf, sizeof(uart_buf)); + + if (bytes_read > 0) { + _uart_port_bridge->uart_write(uart_buf, bytes_read); + } + + px4_usleep(10000); + } + } + + } else { + if (_uart_port_bridge->is_open()) { + PX4_INFO("Closed UART ESC Bridge device"); + _uart_port_bridge->uart_close(); + } + } + + /* Don't process commands if outputs on */ + if (!_outputs_on) { + if (_current_cmd.valid()) { + //PX4_INFO("sending %d commands with delay %dus",_current_cmd.repeats,_current_cmd.repeat_delay_us); + flushUartRx(); + + do { + //PX4_INFO("CMDs left %d",_current_cmd.repeats); + if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { + if (_current_cmd.repeats == 0) { + _current_cmd.clear(); + } + + if (_current_cmd.response) { + readResponse(&_current_cmd); + } + + } else { + if (_current_cmd.retries == 0) { + _current_cmd.clear(); + PX4_ERR("Failed to send command, errno: %i", errno); + + } else { + _current_cmd.retries--; + PX4_ERR("Failed to send command, errno: %i", errno); + } + } + + px4_usleep(_current_cmd.repeat_delay_us); + } while (_current_cmd.repeats-- > 0); + + } else { + Command *new_cmd = _pending_cmd.load(); + + if (new_cmd) { + _current_cmd = *new_cmd; + _pending_cmd.store(nullptr); + } + } + } + + /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ + _mixing_output.updateSubscriptions(true); + + perf_end(_cycle_perf); +} + +int ModalaiEsc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for... + +### Implementation +By default the module runs on a work queue with a callback on the uORB actuator_controls topic. + +### Examples +It is typically started with: +$ todo + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("modalai_esc", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Send reset request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("version", "Send version request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("version-ext", "Send extended version request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("rpm", "Closed-Loop RPM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, -32768, 32768, "RPM, -32,768 to 32,768", false); + PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); + PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); + PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); + PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("tone", "Send tone generation request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 255, "Period of sound, inverse frequency, 0-255", false); + PRINT_MODULE_USAGE_PARAM_INT('d', 0, 0, 255, "Duration of the sound, 0-255, 1LSB = 13ms", false); + PRINT_MODULE_USAGE_PARAM_INT('v', 0, 0, 100, "Power (volume) of sound, 0-100", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("led", "Send LED control request"); + PRINT_MODULE_USAGE_PARAM_INT('l', 0, 0, 4095, "Bitmask 0x0FFF (12 bits) - ESC0 (RGB) ESC1 (RGB) ESC2 (RGB) ESC3 (RGB)", false); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int ModalaiEsc::print_status() +{ + PX4_INFO("Max update rate: %i Hz", _current_update_rate); + PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); + PX4_INFO("UART port: %s", _device); + PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); + + PX4_INFO(""); + + PX4_INFO("Params: UART_ESC_CONFIG: % " PRIi32, _parameters.config); + PX4_INFO("Params: UART_ESC_BAUD: % " PRIi32, _parameters.baud_rate); + PX4_INFO("Params: UART_ESC_MOTOR1: % " PRIi32, _parameters.motor_map[0]); + PX4_INFO("Params: UART_ESC_MOTOR2: % " PRIi32, _parameters.motor_map[1]); + PX4_INFO("Params: UART_ESC_MOTOR3: % " PRIi32, _parameters.motor_map[2]); + PX4_INFO("Params: UART_ESC_MOTOR4: % " PRIi32, _parameters.motor_map[3]); + PX4_INFO("Params: UART_ESC_RPM_MIN: % " PRIi32, _parameters.rpm_min); + PX4_INFO("Params: UART_ESC_RPM_MAX: % " PRIi32, _parameters.rpm_max); + + PX4_INFO(""); + + for( int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++){ + PX4_INFO("-- ID: %i", i); + PX4_INFO(" State: %i", _esc_chans[i].state); + PX4_INFO(" Requested: %i RPM", _esc_chans[i].rate_req); + PX4_INFO(" Measured: %i RPM", _esc_chans[i].rate_meas); + PX4_INFO(" Command Counter: %i", _esc_chans[i].cmd_counter); + PX4_INFO(" Voltage: %f VDC", (double)_esc_chans[i].voltage); + PX4_INFO(""); + } + + perf_print_counter(_cycle_perf); + perf_print_counter(_output_update_perf); + + _mixing_output.printStatus(); + + return 0; +} + +extern "C" __EXPORT int modalai_esc_main(int argc, char *argv[]) +{ + return ModalaiEsc::main(argc, argv); +} diff --git a/src/drivers/actuators/modalai_esc/modalai_esc.hpp b/src/drivers/actuators/modalai_esc/modalai_esc.hpp new file mode 100644 index 0000000000..458c566a58 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/modalai_esc.hpp @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 +#include +#include + +#include "modalai_esc_serial.hpp" + +#include "qc_esc_packet.h" +#include "qc_esc_packet_types.h" + +class ModalaiEsc : public cdev::CDev, public ModuleBase, public OutputModuleInterface +{ +public: + ModalaiEsc(); + virtual ~ModalaiEsc(); + + /** @see ModuleBase */ + static int task_spawn(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; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + /** @see OutputModuleInterface */ + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + /** @see OutputModuleInterface */ + void mixerChanged() override; + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + typedef enum { + UART_ESC_RESET, + UART_ESC_VERSION, + UART_ESC_TONE, + UART_ESC_LED + } uart_esc_cmd_t; + + struct Command { + uint16_t id = 0; + uint8_t len = 0; + uint16_t repeats = 0; + uint16_t repeat_delay_us = 2000; + uint8_t retries = 0; + bool response = false; + uint16_t resp_delay_us = 1000; + bool print_feedback = false; + + static const uint8_t BUF_SIZE = 128; + uint8_t buf[BUF_SIZE]; + + bool valid() const { return len > 0; } + void clear() { len = 0; } + }; + + int sendCommandThreadSafe(Command *cmd); + +private: + static constexpr uint32_t MODALAI_ESC_UART_CONFIG = 1; + static constexpr uint32_t MODALAI_ESC_DEFAULT_BAUD = 250000; + static constexpr uint16_t MODALAI_ESC_OUTPUT_CHANNELS = 4; + static constexpr uint16_t MODALAI_ESC_OUTPUT_DISABLED = 0; + + static constexpr uint32_t MODALAI_ESC_WRITE_WAIT_US = 200; + static constexpr uint32_t MODALAI_ESC_DISCONNECT_TIMEOUT_US = 500000; + + static constexpr uint16_t DISARMED_VALUE = 0; + + static constexpr uint16_t MODALAI_ESC_PWM_MIN = 0; + static constexpr uint16_t MODALAI_ESC_PWM_MAX = 800; + static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MIN = 5000; + static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MAX = 17000; + + static constexpr float MODALAI_ESC_MODE_DISABLED_SETPOINT = -0.1f; + static constexpr float MODALAI_ESC_MODE_THRESHOLD = 0.0f; + + static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_MIN = 0.0f; + static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_MAX = 1.0f; + static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_1 = 0.30f; + static constexpr float MODALAI_ESC_MODE_DEAD_ZONE_2 = 0.02f; + + static constexpr uint32_t MODALAI_ESC_MODE = 0; + static constexpr uint32_t MODALAI_ESC_MODE_TURTLE_AUX1 = 1; + static constexpr uint32_t MODALAI_ESC_MODE_TURTLE_AUX2 = 2; + static constexpr uint32_t MODALAI_ESC_MODE_UART_BRIDGE = 3; + + //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODALAI_ESC_PWM_MAX); } + //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODALAI_ESC_RPM_MAX); } + + ModalaiEscSerial *_uart_port; + ModalaiEscSerial *_uart_port_bridge; + + typedef struct { + int32_t config{MODALAI_ESC_UART_CONFIG}; + int32_t mode{MODALAI_ESC_MODE}; + float dead_zone_1{MODALAI_ESC_MODE_DEAD_ZONE_1}; + float dead_zone_2{MODALAI_ESC_MODE_DEAD_ZONE_2}; + int32_t baud_rate{MODALAI_ESC_DEFAULT_BAUD}; + int32_t rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN}; + int32_t rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX}; + int32_t motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; + } uart_esc_params_t; + + struct EscChan { + int16_t rate_req; + uint8_t state; + uint16_t rate_meas; + uint8_t power_applied; + uint8_t led; + uint8_t cmd_counter; + float voltage; //Volts + float current; //Amps + float temperature; //deg C + hrt_abstime feedback_time; + }; + + typedef struct { + uint8_t number; + int8_t direction; + } ch_assign_t; + + typedef struct { + led_control_s control{}; + vehicle_control_mode_s mode{}; + uint8_t led_mask;// TODO led_mask[MODALAI_ESC_OUTPUT_CHANNELS]; + bool breath_en; + uint8_t breath_counter; + bool test; + } led_rsc_t; + + ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; + MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + int _class_instance{-1}; + + perf_counter_t _cycle_perf; + perf_counter_t _output_update_perf; + + bool _outputs_on{false}; + + unsigned _current_update_rate{0}; + + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + + //uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; + + uart_esc_params_t _parameters; + int update_params(); + int load_params(uart_esc_params_t *params, ch_assign_t *map); + + bool _turtle_mode_en{false}; + int32_t _rpm_fullscale{0}; + manual_control_setpoint_s _manual_control_setpoint{}; + + uint16_t _cmd_id{0}; + Command _current_cmd; + px4::atomic _pending_cmd{nullptr}; + + EscChan _esc_chans[MODALAI_ESC_OUTPUT_CHANNELS] {}; + Command _esc_cmd; + esc_status_s _esc_status; + EscPacket _fb_packet; + EscPacket _uart_bridge_packet; + + led_rsc_t _led_rsc; + int _fb_idx; + + static const uint8_t READ_BUF_SIZE = 128; + uint8_t _read_buf[READ_BUF_SIZE]; + + void updateLeds(vehicle_control_mode_s mode, led_control_s control); + + int populateCommand(uart_esc_cmd_t cmd_type, uint8_t cmd_mask, Command *out_cmd); + int readResponse(Command *out_cmd); + int parseResponse(uint8_t *buf, uint8_t len, bool print_feedback); + int flushUartRx(); + int checkForEscTimeout(); +}; diff --git a/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp b/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp new file mode 100644 index 0000000000..93ab40c7bb --- /dev/null +++ b/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "string.h" +#include "modalai_esc_serial.hpp" + +ModalaiEscSerial::ModalaiEscSerial() +{ +} + +ModalaiEscSerial::~ModalaiEscSerial() +{ + if (_uart_fd >= 0) { + uart_close(); + } +} + +int ModalaiEscSerial::uart_open(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port in use: %s (%i)", dev, errno); + return -1; + } + + /* Open UART */ + _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + + if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { + PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); + uart_close(); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &_cfg); + + /* Disable output post-processing */ + _cfg.c_oflag &= ~OPOST; + + _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + _cfg.c_cflag &= ~CSIZE; + _cfg.c_cflag |= CS8; /* 8-bit characters */ + _cfg.c_cflag &= ~PARENB; /* no parity bit */ + _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ + _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + + /* setup for non-canonical mode */ + _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + + if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { + PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); + uart_close(); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { + PX4_ERR("Error configuring port: %s (tcsetattr)", dev); + uart_close(); + return -1; + } + + _speed = speed; + + return 0; +} + +int ModalaiEscSerial::uart_set_baud(speed_t speed) +{ + if (_uart_fd < 0) { + return -1; + } + + if (cfsetispeed(&_cfg, speed) < 0) { + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { + return -1; + } + + _speed = speed; + + return 0; +} + +int ModalaiEscSerial::uart_close() +{ + if (_uart_fd < 0) { + PX4_ERR("invalid state for closing"); + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { + PX4_ERR("failed restoring uart to original state"); + } + + if (close(_uart_fd)) { + PX4_ERR("error closing uart"); + } + + _uart_fd = -1; + + return 0; +} + +int ModalaiEscSerial::uart_write(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + + return write(_uart_fd, buf, len); +} + +int ModalaiEscSerial::uart_read(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + + return read(_uart_fd, buf, len); +} diff --git a/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp b/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp new file mode 100644 index 0000000000..fdecbddc4f --- /dev/null +++ b/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + + +class ModalaiEscSerial +{ +public: + ModalaiEscSerial(); + virtual ~ModalaiEscSerial(); + + int uart_open(const char *dev, speed_t speed); + int uart_set_baud(speed_t speed); + int uart_close(); + int uart_write(FAR void *buf, size_t len); + int uart_read(FAR void *buf, size_t len); + bool is_open() { return _uart_fd >= 0; }; + int uart_get_baud() {return _speed; } + +private: + int _uart_fd = -1; + struct termios _orig_cfg; + struct termios _cfg; + int _speed = -1; +}; diff --git a/src/drivers/actuators/modalai_esc/parameters.c b/src/drivers/actuators/modalai_esc/parameters.c new file mode 100644 index 0000000000..3c7c21a66e --- /dev/null +++ b/src/drivers/actuators/modalai_esc/parameters.c @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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. + * + ****************************************************************************/ + +/** + * UART ESC configuration + * + * Selects what type of UART ESC, if any, is being used. + * + * @reboot_required true + * + * @group UART ESC + * @value 0 - Disabled + * @value 1 - VOXL ESC + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0); + +/** + * UART ESC baud rate + * + * Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products. + * + * @group UART ESC + * @unit bit/s + */ +PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); + +/** + * Motor mappings for ModalAI ESC M004 + * + * HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed) + * 4(1) 3(4) + * [front] + * 1(3) 2(2) + */ + +/** + * UART ESC Motor 1 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3); + +/** + *UART ESC Motor 2 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); + +/** + * UART ESC Motor 3 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4); + +/** + * UART ESC Motor 4 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1); + +/** + * UART ESC RPM Min + * + * Minimum RPM for ESC + * + * @group UART ESC + * @unit RPM + */ +PARAM_DEFINE_INT32(UART_ESC_RPM_MIN, 5500); + +/** + * UART ESC RPM Max + * + * Maximum RPM for ESC + * + * @group UART ESC + * @unit RPM + */ +PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000); + +/** + * UART ESC Mode + * + * Selects what type of mode is enabled, if any + * + * @reboot_required true + * + * @group UART ESC + * @value 0 - None + * @value 1 - Turtle Mode enabled via AUX1 + * @value 2 - Turtle Mode enabled via AUX2 + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(UART_ESC_MODE, 0); + +/** + * UART ESC Mode Deadzone 1. + * + * Must be greater than Deadzone 2. + * Absolute value of stick position needed to activate a motor. + * + * @group UART ESC Mode Deadzone 1 + * @min 0.01 + * @max 0.99 + * @decimal 10 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(UART_ESC_DEAD1, 0.30f); + +/** + * UART ESC Mode Deadzone 2. + * + * Must be less than Deadzone 1. + * Absolute value of stick position considered no longer on the X or Y axis, + * thus targetting a specific motor (single). + * + * @group UART ESC Mode Deadzone 2 + * @min 0.01 + * @max 0.99 + * @decimal 10 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(UART_ESC_DEAD2, 0.02f); diff --git a/src/drivers/actuators/modalai_esc/qc_esc_packet.c b/src/drivers/actuators/modalai_esc/qc_esc_packet.c new file mode 100644 index 0000000000..0dc8fab664 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/qc_esc_packet.c @@ -0,0 +1,254 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include "crc16.h" +#include "qc_esc_packet.h" +#include "qc_esc_packet_types.h" + +#include + +int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_packet(ESC_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size); +} + +int32_t qc_esc_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_packet(ESC_PACKET_TYPE_VERSION_EXT_REQUEST, &id, 1, out, out_size); +} + +int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + char payload[] = "RESET0"; + payload[5] += id; + + return qc_esc_create_packet(ESC_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size); +} + + +int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size) +{ + uint8_t data[4] = {frequency, duration, power, mask}; + return qc_esc_create_packet(ESC_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size); +} + +int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size) +{ + uint8_t data[2] = {led_byte_1, led_byte_2}; + return qc_esc_create_packet(ESC_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size); +} + +int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_packet(ESC_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size); +} + +int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //limit the pwm commands + + if (pwm0 > 800) { pwm0 = 800; } if (pwm0 < -800) { pwm0 = -800; } + + if (pwm1 > 800) { pwm1 = 800; } if (pwm1 < -800) { pwm1 = -800; } + + if (pwm2 > 800) { pwm2 = 800; } if (pwm2 < -800) { pwm2 = -800; } + + if (pwm3 > 800) { pwm3 = 800; } if (pwm3 < -800) { pwm3 = -800; } + + //least significant bit is used for feedback request + pwm0 &= ~(0x0001); pwm1 &= ~(0x0001); pwm2 &= ~(0x0001); pwm3 &= ~(0x0001); + + if (fb_id == 0) { pwm0 |= 0x0001; } if (fb_id == 1) { pwm1 |= 0x0001; } + + if (fb_id == 2) { pwm2 |= 0x0001; } if (fb_id == 3) { pwm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = pwm0; data[1] = pwm1; data[2] = pwm2; data[3] = pwm3; data[4] = leds; + return qc_esc_create_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + + +int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //least significant bit is used for feedback request + rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001); + + if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; } + + if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; + return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + +int32_t qc_esc_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) +{ + uint16_t packet_size = size + 5; + + if (packet_size > 255) { return -1; } + + if (out_size < packet_size) { return -2; } + + out[0] = 0xAF; + out[1] = packet_size; + out[2] = type; + + memcpy(&(out[3]), data, size); + + uint16_t crc = crc16_init(); + crc = crc16(crc, &(out[1]), packet_size - 3); + + memcpy(&(out[packet_size - 2]), &crc, sizeof(uint16_t)); + + return packet_size; +} + + + + +//feed in a character and see if we got a complete packet +int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet) +{ + int16_t ret = ESC_NO_PACKET; + + uint16_t chk_comp; + uint16_t chk_rcvd; + + if (packet->len_received >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + } + + //reset the packet and start parsing from beginning if length byte == header + //this can only happen if the packet is de-synced and last char of checksum + //ends up being equal to the header, in that case we can end up in endless loop + //unable to re-sync with the packet + if (packet->len_received == 1 && c == ESC_PACKET_HEADER) { + packet->len_received = 0; + } + + switch (packet->len_received) { + case 0: //header + packet->bp = packet->buffer; //reset the pointer for storing data + qc_esc_packet_checksum_reset(packet); //reset the checksum to starting value + + if (c != ESC_PACKET_HEADER) { //check the packet header + packet->len_received = 0; + ret = ESC_ERROR_BAD_HEADER; + break; + } + + packet->len_received++; + *(packet->bp)++ = c; + break; + + case 1: //length + packet->len_received++; + *(packet->bp)++ = c; + packet->len_expected = c; + + if (packet->len_expected >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + ret = ESC_ERROR_BAD_LENGTH; + break; + } + + qc_esc_packet_checksum_process_char(packet, c); + break; + + default: //rest of the packet + packet->len_received++; + *(packet->bp)++ = c; + + if (packet->len_received < (packet->len_expected - 1)) { //do not compute checksum of checksum (last 2 bytes) + qc_esc_packet_checksum_process_char(packet, c); + } + + if (packet->len_received < packet->len_expected) { //waiting for more bytes + break; + } + + //grab the computed checksum and compare against the received value + chk_comp = qc_esc_packet_checksum_get(packet); + + memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t)); + + if (chk_comp == chk_rcvd) { ret = packet->len_received; } + + else { ret = ESC_ERROR_BAD_CHECKSUM; } + + packet->len_received = 0; + break; + } + + return ret; +} diff --git a/src/drivers/actuators/modalai_esc/qc_esc_packet.h b/src/drivers/actuators/modalai_esc/qc_esc_packet.h new file mode 100644 index 0000000000..8af2b6ea54 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/qc_esc_packet.h @@ -0,0 +1,275 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for Electronic Speed Controller (ESC) UART interface + */ + +#ifndef QC_ESC_PACKET +#define QC_ESC_PACKET + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "crc16.h" + +// Define byte values that correspond to setting red, greed, and blue LEDs +#define QC_ESC_LED_RED_ON 1 +#define QC_ESC_LED_GREEN_ON 2 +#define QC_ESC_LED_BLUE_ON 4 + + +// Header of the packet. Each packet must start with this header +#define ESC_PACKET_HEADER 0xAF + +enum { ESC_ERROR_BAD_LENGTH = -3, + ESC_ERROR_BAD_HEADER = -2, + ESC_ERROR_BAD_CHECKSUM = -1, + ESC_NO_PACKET = 0 + }; + +// Defines for the constatnt offsets of different parts of the packet +enum { ESC_PACKET_POS_HEADER1 = 0, + ESC_PACKET_POS_LENGTH, + ESC_PACKET_POS_TYPE, + ESC_PACKET_POS_DATA + }; + +// Definition of the structure that holds the state of the incoming data that is being recived (i.e. incomplete packets) +typedef struct { + uint8_t len_received; // Number of chars received so far + uint8_t len_expected; // Expected number of chars based on header + uint8_t *bp; // Pointer to the next write position in the buffer + uint16_t crc; // Accumulated CRC value so far + uint8_t buffer[64]; // Buffer to hold incoming data that is being parsed +} EscPacket; + + +// Definition of the response packet from ESC, containing the ESC version information +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE + + uint8_t id; // ID of the ESC that responded + uint16_t sw_version; // Software version of the ESC firmware + uint16_t hw_version; // HW version of the board + + uint32_t unique_id; // Unique ID of the ESC, if available + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_VERSION_INFO; + +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; + uint16_t sw_version; + uint16_t hw_version; + uint8_t unique_id[12]; + char firmware_git_version[12]; + char bootloader_git_version[12]; + uint16_t bootloader_version; + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_EXTENDED_VERSION_INFO; + +// Definition of the feedback response packet from ESC +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE + + uint8_t state; // bits 0:3 = state, bits 4:7 = ID + uint16_t rpm; // Current RPM of the motor + uint8_t cmd_counter; // Number of commands received by the ESC + uint8_t reserved0; + int8_t voltage; // Voltage = (-28)/34.0 + 9.0 = 8.176V. 0xE4 --> 228 (-28) + uint8_t reserved1; + + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_RESPONSE; + +// Definition of the feedback response packet from ESC +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_FB_RESPONSE + uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID + + uint16_t rpm; // Current RPM of the motor + uint8_t cmd_counter; // Number of commands received by the ESC + uint8_t power; // Applied power [0..100] + + uint16_t voltage; // Voltage measured by the ESC in mV + int16_t current; // Current measured by the ESC in 8mA resolution + int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution + + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_RESPONSE_V2; + + +//------------------------------------------------------------------------- +//Below are functions for generating packets that would be outgoing to ESCs +//------------------------------------------------------------------------- + +// Create a generic packet by providing all required components +// Inputs are packet type, input data array and its size, output array and maximum size of output array +// Resulting packet will be placed in the output data array together with appropriate header and checksum +// Output value represents total length of the created packet (if positive), otherwise error code +int32_t qc_esc_create_packet(uint8_t type, uint8_t *input_data, uint16_t input_size, uint8_t *out_data, + uint16_t out_data_size); + +// Create a packet for requesting version information from ESC with desired id +// If an ESC with this id is connected and receives this command, it will reply with it's version information +int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); +int32_t qc_esc_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for requesting an ESC with desired id to reset +// When ESC with the particular id receives this command, and it's not spinning, ESC will reset +// This is useful for entering bootloader without removing power from the system +int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for generating a tone packet (signals are applied to motor to generate sounds) +// Inputs are relative frequency (0-255), relative duration (0-255), power (0-255) and bit mask for which ESCs should play a tone +// Bit mask definition: if bit i is set to 1, then ESC with ID=i will generate the tone +// Note that tones can only be generated when motor is not spinning +int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size); + +// Create a packet for standalone LED control +// Bit mask definition: +// led_byte_1 - bit0 = ESC0 Red, bit1 = ESC0, Green, bit2 = ESC0 Blue, bit3 = ESC1 Red, bit4 = ESC1 Green, +// bit5 = ESC1 Blue, bit6 = ESC2 Red, bit7 = ESC2 Green +// led_byte_2 - bit0 = ESC2 Blue, bit1 = ESC3 Red, bit2 = ESC3 Green, bit3 = ESC3 Blue, bits 4:7 = unused +// Note that control can only be sent when motor is not spinning +int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size); + +// Create a packet for setting the ID of an ESC +// Return value is the length of generated packet (if positive), otherwise error code +// Note that all ESCs that will receive this command will be set to this ID +int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + + +//------------------------------------------------------------------------- +// Below are functions for processing incoming packets +//------------------------------------------------------------------------- + + +// Feed one char and see if we have accumulated a complete packet +int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet); + +// Get a pointer to the packet type from a pointer to EscPacket +static inline uint8_t qc_esc_packet_get_type(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_TYPE]; } + +// Get a pointer to the packet type from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t qc_esc_packet_raw_get_type(uint8_t *packet) { return packet[ESC_PACKET_POS_TYPE]; } + +//get a pointer to the packet payload from a pointer to EscPacket +static inline uint8_t *qc_esc_packet_get_data_ptr(EscPacket *packet) { return &(packet->buffer[ESC_PACKET_POS_DATA]); } + +// Get a pointer to the packet payload from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t *qc_esc_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[ESC_PACKET_POS_DATA]); } + +// Get the total size (length) in bytes of the packet +static inline uint8_t qc_esc_packet_get_size(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_LENGTH]; } + +// Get checksum of the packet from a pointer to EscPacket +static inline uint16_t qc_esc_packet_checksum_get(EscPacket *packet) { return packet->crc; } + +// Calculate the checksum of a data array. Used for packet generation / processing +static inline uint16_t qc_esc_packet_checksum(uint8_t *buf, uint16_t size) +{ + uint16_t crc = crc16_init(); + return crc16(crc, buf, size); +} + +// Reset the checksum of the incoming packet. Used internally for packet reception +static inline void qc_esc_packet_checksum_reset(EscPacket *packet) { packet->crc = crc16_init(); } + +// Process one character for checksum calculation while receiving a packet (used internally for packet reception) +static inline void qc_esc_packet_checksum_process_char(EscPacket *packet, uint8_t c) +{ + packet->crc = crc16_byte(packet->crc, c); +} + + +// Initialize an instance of an EscPacket. This should be called once before using an instance of EscPacket +static inline void qc_esc_packet_init(EscPacket *packet) +{ + packet->len_received = 0; + packet->len_expected = 0; + packet->bp = 0; + + qc_esc_packet_checksum_reset(packet); +} + +// Reset status of the packet that is being parsed. Effectively, this achieves the same thing as _packet_init +// so that _packet_init may be redundant +static inline void qc_esc_packet_reset(EscPacket *packet) +{ + packet->len_received = 0; +} + +#endif //QC_ESC_PACKET + +#ifdef __cplusplus +} +#endif diff --git a/src/drivers/actuators/modalai_esc/qc_esc_packet_types.h b/src/drivers/actuators/modalai_esc/qc_esc_packet_types.h new file mode 100644 index 0000000000..2453d7dbf0 --- /dev/null +++ b/src/drivers/actuators/modalai_esc/qc_esc_packet_types.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains the type values of all supported ESC UART packets + */ + +#ifndef QC_ESC_PACKET_TYPES +#define QC_ESC_PACKET_TYPES + +#define ESC_PACKET_TYPE_VERSION_REQUEST 0 +#define ESC_PACKET_TYPE_PWM_CMD 1 +#define ESC_PACKET_TYPE_RPM_CMD 2 +#define ESC_PACKET_TYPE_SOUND_CMD 3 +#define ESC_PACKET_TYPE_STEP_CMD 4 +#define ESC_PACKET_TYPE_LED_CMD 5 +#define ESC_PACKET_TYPE_RESET_CMD 10 +#define ESC_PACKET_TYPE_SET_ID_CMD 11 +#define ESC_PACKET_TYPE_SET_DIR_CMD 12 +#define ESC_PACKET_TYPE_CONFIG_BOARD_REQUEST 20 +#define ESC_PACKET_TYPE_CONFIG_USER_REQUEST 21 +#define ESC_PACKET_TYPE_CONFIG_UART_REQUEST 22 +#define ESC_PACKET_TYPE_CONFIG_TUNE_REQUEST 23 +#define ESC_PACKET_TYPE_VERSION_EXT_REQUEST 24 + +#define ESC_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use + +#define ESC_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70 +#define ESC_PACKET_TYPE_EEPROM_READ_UNLOCK 71 +#define ESC_PACKET_TYPE_EEPROM_WRITE 72 + +#define ESC_PACKET_TYPE_VERSION_RESPONSE 109 +#define ESC_PACKET_TYPE_PARAMS 110 +#define ESC_PACKET_TYPE_BOARD_CONFIG 111 +#define ESC_PACKET_TYPE_USER_CONFIG 112 +#define ESC_PACKET_TYPE_UART_CONFIG 113 +#define ESC_PACKET_TYPE_TUNE_CONFIG 114 +#define ESC_PACKET_TYPE_FB_RESPONSE 128 +#define ESC_PACKET_TYPE_VERSION_EXT_RESPONSE 131 + +#endif diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index d3fb119757..1ed72860a6 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -364,6 +364,7 @@ class SourceParser(object): 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', 'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C', 'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N', + 'RPM', 'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD','']) for group in self.GetParamGroups(): for param in group.GetParams():