mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
WIP: TMotor Alpha ESC telemetry driver
This commit is contained in:
Daniel Agar
parent
82c1ffb8f8
commit
42a9e540a8
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
)
|
||||
5
src/drivers/tmotor_alpha_esc/Kconfig
Normal file
5
src/drivers/tmotor_alpha_esc/Kconfig
Normal file
@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_TMOTOR_ALPHA_ESC
|
||||
bool "tmotor_alpha_esc"
|
||||
default n
|
||||
---help---
|
||||
Enable support for TMotor Alpha ESC
|
||||
292
src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.cpp
Normal file
292
src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.cpp
Normal file
@ -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 <termios.h>
|
||||
|
||||
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", "<file:dev>", "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);
|
||||
}
|
||||
91
src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.hpp
Normal file
91
src/drivers/tmotor_alpha_esc/TmotorAlphaEsc.hpp
Normal file
@ -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 <float.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class TmotorAlphaEsc : public ModuleBase<TmotorAlphaEsc>, 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_s> _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
|
||||
};
|
||||
105
src/drivers/tmotor_alpha_esc/datalink_driver.py
Normal file
105
src/drivers/tmotor_alpha_esc/datalink_driver.py
Normal file
@ -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__)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
7
src/drivers/tmotor_alpha_esc/module.yaml
Normal file
7
src/drivers/tmotor_alpha_esc/module.yaml
Normal file
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user