diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 4348d736c7..2a4559aa9c 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -12,10 +12,10 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y -CONFIG_COMMON_HYGROMETERS=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_COMMON_HYGROMETERS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_BOSCH_BMI055=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y @@ -41,6 +41,7 @@ CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TMOTOR_ALPHA_ESC=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6 @@ -58,6 +59,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 @@ -81,7 +83,6 @@ 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_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 878bfd79f7..c1610a6b52 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -44,6 +44,7 @@ CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TMOTOR_ALPHA_ESC=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_MODULES_AIRSPEED_SELECTOR=y @@ -60,6 +61,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 @@ -81,7 +83,6 @@ 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_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index dd504480d6..7eb1e86fc3 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -5,6 +5,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_DRIVERS_RPM_RPM_SIMULATOR=y +CONFIG_DRIVERS_TMOTOR_ALPHA_ESC=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y @@ -19,6 +20,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 @@ -43,7 +45,6 @@ CONFIG_MODULES_SIMULATOR=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_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_DYN=y diff --git a/src/drivers/telemetry/CMakeLists.txt b/src/drivers/tmotor_alpha_esc/CMakeLists.txt similarity index 86% rename from src/drivers/telemetry/CMakeLists.txt rename to src/drivers/tmotor_alpha_esc/CMakeLists.txt index 0980f2d9f3..70c23f81b6 100644 --- a/src/drivers/telemetry/CMakeLists.txt +++ b/src/drivers/tmotor_alpha_esc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 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 @@ -30,8 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -add_subdirectory(bst) -add_subdirectory(frsky_telemetry) -add_subdirectory(hott) -#add_subdirectory(iridiumsbd) +px4_add_module( + MODULE drivers__tmotor_alpha_esc + MAIN tmotor_alpha_esc + COMPILE_FLAGS + SRCS + TmotorAlphaEsc.cpp + TmotorAlphaEsc.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/tmotor_alpha_esc/Kconfig b/src/drivers/tmotor_alpha_esc/Kconfig new file mode 100644 index 0000000000..219befc881 --- /dev/null +++ b/src/drivers/tmotor_alpha_esc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_TMOTOR_ALPHA_ESC + bool "tmotor_alpha_esc" + default n + ---help--- + Enable support for TMotor Alpha ESC diff --git a/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.cpp b/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.cpp new file mode 100644 index 0000000000..437316d5fe --- /dev/null +++ b/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.cpp @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "TmotorAlphaEsc.hpp" + +#include + +using namespace time_literals; + +TmotorAlphaEsc::TmotorAlphaEsc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)) +{ + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; +} + +TmotorAlphaEsc::~TmotorAlphaEsc() +{ + perf_free(_cycle_perf); +} + +int TmotorAlphaEsc::init() +{ + // status + int ret = 0; + + do { // create a scope to handle exit conditions using break + + // open fd + _fd = ::open(_device, O_RDWR | O_NOCTTY); + + if (_fd < 0) { + PX4_ERR("Error opening fd"); + return -1; + } + + // baudrate 115200, 8 bits, no parity, 1 stop bit + unsigned speed = B115200; + termios uart_config{}; + int termios_state{}; + + tcgetattr(_fd, &uart_config); + + // clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; + + // set baud rate + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d ISPD", termios_state); + ret = -1; + break; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD\n", termios_state); + ret = -1; + break; + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + ret = -1; + break; + } + + uart_config.c_cflag |= (CLOCAL | CREAD); // ignore modem controls + uart_config.c_cflag &= ~CSIZE; + uart_config.c_cflag |= CS8; // 8-bit characters + uart_config.c_cflag &= ~PARENB; // no parity bit + uart_config.c_cflag &= ~CSTOPB; // only need 1 stop bit + uart_config.c_cflag &= ~CRTSCTS; // no hardware flowcontrol + + // setup for non-canonical mode + uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + uart_config.c_oflag &= ~OPOST; + + // fetch bytes as they become available + uart_config.c_cc[VMIN] = 1; + uart_config.c_cc[VTIME] = 1; + + if (_fd < 0) { + PX4_ERR("FAIL: laser fd"); + ret = -1; + break; + } + } while (0); + + return ret; +} + +int TmotorAlphaEsc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + TmotorAlphaEsc *instance = new TmotorAlphaEsc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleOnInterval(_current_update_interval); + + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); + + } else { + PX4_INFO("valid device required"); + } + } + + return PX4_ERROR; +} + +void TmotorAlphaEsc::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + if (!_initialized) { + if (init() == PX4_OK) { + _initialized = true; + + } else { + PX4_ERR("init failed"); + exit_and_cleanup(); + } + + } else { + + perf_begin(_cycle_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + uint8_t buffer[120]; + + int newBytes = ::read(_fd, &buffer[0], sizeof(buffer)); + + if (newBytes > 0) { + _bytes_rx += newBytes; + } + + + + // TODO: + esc_status_s esc_status{}; + + + // self.motor_ix = data[0] + // self.channel_bag_number = int.from_bytes(data[1:3], 'big') + // self.rx_throttle = int.from_bytes(data[3:5], 'big')*(100/1024) + // self.actual_throttle = int.from_bytes(data[5:7], 'big')*(100/1024) + // self.electric_rpm = int(int.from_bytes(data[7:9], 'big')*(10/pole_pairs)) + // self.bus_voltage = int.from_bytes(data[9:11], 'big')/10 + // self.bus_current = round(c_int16(int.from_bytes(data[11:13], 'big')).value/64, 1) + // self.phase_current = round(c_int16(int.from_bytes(data[13:15], 'big')).value/64, 1) + // self.mos_temperature = get_temp(data[15]) + // self.cap_temperature = get_temp(data[16]) + // self.status_byte = data[17:] + // # status_bits = "{:08b}".format(int(status_byte.hex(),16)) + + + esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(esc_status); + + + + perf_end(_cycle_perf); + + ScheduleDelayed(100_ms); + } +} + +int TmotorAlphaEsc::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int TmotorAlphaEsc::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + perf_print_counter(_cycle_perf); + + return 0; +} + +int +TmotorAlphaEsc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("tmotor_alpha_esc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "device", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int tmotor_alpha_esc_main(int argc, char *argv[]) +{ + return TmotorAlphaEsc::main(argc, argv); +} diff --git a/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.hpp b/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.hpp new file mode 100644 index 0000000000..b7aaf2a0e7 --- /dev/null +++ b/src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class TmotorAlphaEsc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + TmotorAlphaEsc(const char *device); + virtual ~TmotorAlphaEsc(); + + /** @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::print_status() */ + int print_status() override; + + int init(); + +private: + void Run() override; + + static constexpr unsigned _current_update_interval{100000}; // 10 Hz + + uORB::PublicationMultiData _esc_status_pub{ORB_ID(esc_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + uint32_t _bytes_rx{0}; + + bool _initialized{false}; + + int _fd{-1}; + char _device[20] {}; ///< device / serial port path +}; diff --git a/src/drivers/tmotor_alpha_esc/datalink_driver.py b/src/drivers/tmotor_alpha_esc/datalink_driver.py new file mode 100644 index 0000000000..3e8c65dbd8 --- /dev/null +++ b/src/drivers/tmotor_alpha_esc/datalink_driver.py @@ -0,0 +1,105 @@ +#!/home/eduard/.pyenv/versions/bc3100/bin/python +import serial +from time import sleep +from ctypes import c_int16 + + +ser = serial.Serial ("/dev/ttyUSB0", 115200) #Open port with baud rate +i=1 +telem_header = int.from_bytes(b'\x9b', 'big') + +n_poles = 28 +pole_pairs = n_poles/2 + +temp_table = { + 241:0, 240:1, 239:2, 238:3, 237:4, 236:5, 235:6, 234:7, 233:8, 232:9, + 231:10, 230:11, 229:12, 228:13, 227:14, 226:15, 224:16, 223:17, 222:18, 220:19, + 219:20, 217:21, 216:22, 214:23, 213:24, 211:25, 209:26, 208:27, 206:28, 204:29, + 202:30, 201:31, 199:32, 197:33, 195:34, 193:35, 191:36, 189:37, 187:38, 185:39, + 183:40, 181:41, 179:42, 177:43, 174:44, 172:45, 170:46, 168:47, 166:48, 164:49, + 161:50, 159:51, 157:52, 154:53, 152:54, 150:55, 148:56, 146:57, 143:58, 141:59, + 139:60, 136:61, 134:62, 132:63, 130:64, 128:65, 125:66, 123:67, 121:68, 119:69, + 117:70, 115:71, 113:72, 111:73, 109:74, 106:75, 105:76, 103:77, 101:78, 99:79, + 97:80, 95:81, 93:82, 91:83, 90:84, 88:85, 85:86, 84:87, 82:88, 81:89, + 79:90, 77:91, 76:92, 74:93, 73:94, 72:95, 69:96, 68:97, 66:98, 65:99, + 64:100, 62:101, 62:102, 61:103, 59:104, 58:105, 56:106, 54:107, 54:108, 53:109, + 51:110, 51:111, 50:112, 48:113, 48:114, 46:115, 46:116, 44:117, 43:118, 43:119, + 41:120, 41:121, 39:122, 39:123, 39:124, 37:125, 37:126, 35:127, 35:128, 33:129, +} + +def get_temp(ix): + try: + keys = temp_table.keys() + if ix in keys: + return temp_table[ix] + x1 = min([val for val in keys if val > ix]) + x0 = max([val for val in keys if val < ix]) + y1 = temp_table[x1] + y0 = temp_table[x0] + yix = y0+ ((y1-y0)/(x1-x0)) * (ix-x0) + return yix + except: + return -1 + + + +class EscTelemetryMessage: + def __init__(self, data) -> None: + + self.motor_ix = data[0] + self.channel_bag_number = int.from_bytes(data[1:3], 'big') + self.rx_throttle = int.from_bytes(data[3:5], 'big')*(100/1024) + self.actual_throttle = int.from_bytes(data[5:7], 'big')*(100/1024) + self.electric_rpm = int(int.from_bytes(data[7:9], 'big')*(10/pole_pairs)) + self.bus_voltage = int.from_bytes(data[9:11], 'big')/10 + self.bus_current = round(c_int16(int.from_bytes(data[11:13], 'big')).value/64, 1) + self.phase_current = round(c_int16(int.from_bytes(data[13:15], 'big')).value/64, 1) + self.mos_temperature = get_temp(data[15]) + self.cap_temperature = get_temp(data[16]) + self.status_byte = data[17:] + # status_bits = "{:08b}".format(int(status_byte.hex(),16)) + + + + +class TMotorDataLinkMessage: + def __init__(self, message_bytes) -> None: + # basic checking if it's no good, crash. + assert len(message_bytes)==160 + assert message_bytes[0]==155 # aka 0x9b' + assert message_bytes[1]==158 + + self.esc_messages = [None]*8 + + for start_index in range(6,157, 19): + esc_message = EscTelemetryMessage(message_bytes[start_index:start_index+18]) + self.esc_messages[esc_message.motor_ix-1] = esc_message + + crc_sum = sum(message_bytes[7:158]) + crc = int.from_bytes(message_bytes[158:], 'big') + + print(f'{crc_sum=} {crc=}') # this is clearly still wrong.... + + + +if __name__=="__main__": + + while True: + received_data = ser.read() #read serial port + sleep(0.05) + data_left = ser.in_waiting #check for number of remaining bytes + received_data += ser.read(data_left) + + try: + dl_message = TMotorDataLinkMessage(received_data) + except Exception as e: + print(e) + continue + + for esc_message in dl_message.esc_messages[:4]: + print(esc_message.__dict__) + + + + + diff --git a/src/drivers/tmotor_alpha_esc/module.yaml b/src/drivers/tmotor_alpha_esc/module.yaml new file mode 100644 index 0000000000..3c57070abe --- /dev/null +++ b/src/drivers/tmotor_alpha_esc/module.yaml @@ -0,0 +1,7 @@ +module_name: TMotor Alpha ESC telemetry +serial_config: + - command: tmotor_alpha_esc start -d ${SERIAL_DEV} + port_config_param: + name: SENS_TMESC_CFG + group: Telemetry +