ardupilot
ardupilot copied to clipboard
ArduPilot
Copter: add t-motor "Data Link" ESC telemetry support
Feature request
See here https://discuss.ardupilot.org/t/ac-t-motor-data-link-esc-telemetrie-support/38291
Platform [ ] All [ ] AntennaTracker [ x] Copter [ ] Plane [ ] Rover [ ] Submarine
Just released the 120A Alpha ESC, now I'm really interested in having support for this. http://store-en.tmotor.com/goods.php?id=909
I have the spec if someone wants to implement this.
@IamPete1
I would be interested in learning how you did this. I've been searching everywhere I can think and am coming up empty-handed.
I have the spec if someone wants to implement this.
Hi Peter, Do you mean the specs for the connection between a singel Alpha ESC and the Data Link hardeware (https://store.tmotor.com/goods.php?id=728) or the output of the Data Link device (with the ESCs connected to it) itself. In the latter case you would need only one serial port at Ardupilot flight controller side, otherwise you would have to have one serial port free for each Alpha ESC. This would be helpful for airplanes only, but not for VTOLs or copters. In any case I am also interested in the specs.
Regards Rolf
For a single ESC without T-Motor Data Link, this implementation will work out of the box, as the protocols are identical. You would probably need to change RPM scaling coefficient according to your motor.
I have the spec if someone wants to implement this.
@IamPete1 Could you send the protocol docs to me please? A partner graciously sent me a bunch of motors with Alpha ESCs on them. Depending what the protocol looks like I might be interested in having a hand at making a driver sometime...
@hendjoshsr71 https://wiki.paparazziuav.org/wiki/Alpha_esc_with_telemetry_output#Links
I'm trying to implement something similar on arduino for a test stand application. However, I notice some odd details. The full message frame (screenshot below) is 24 bytes instead of 22. I think the 2 additional bytes are from the 0x01 0x02 after the 0x9B 0x16 headers. I did not see it from the paparazziuav specification so does anyone know what those 2 bytes are? In addition, how are checksum being done, in the screenshot, the last 2 columns are where I display the calculated and expected checksum, respectively. For the expected, I just do a shift to combine the byte. For the calculated, I just add all the buffer element from 0x9B excluding the checksum byte. Is that the correct approach?
I answered my own question. Got the protocol from T-Motor and the 01 and 02 are just for their internal use (versioning). Checksum was also sorted out as well.
@IamPete1 Would you be able to send me the spec that you have for the datalink unit? I'm having a really hard time getting my hands on it..
@eduardkieser You can email T-MOTOR for protocol specs. If anyone wants to try it out on Arduino, here's a working library.
It has a two CAN ports, so maybe it could be connected directly to a ArduPilot CAN port?
@v-petrovic thanks, I finally did manage get my hands on the spec and made a python driver that is able to get the esc-telemetry form the datalink V2 using a raspberry pi. I would of course prefer if this can simply be handeled by the FC and have the esc telemetry end up with the rest of the data logs. I have a cube orange connected to some alpha ESC's via a datalink V2 sitting on my desk, so I'd be happy to help with testing. I'm just too much of a C++ newb and have no adrupilot experience, so making it myself would simply take too long..
@eduardkieser You could add an STM32 based board for converting datalink serial data into DroneCAN ESC status messages supported by ArduPilot. See https://github.com/dronecan/DSDL/blob/master/uavcan/equipment/esc/1034.Status.uavcan.
Can you post the code somewhere? I could try to integrate that in ArduPilot
Sure, its not particularly well implemented, but it allowed us to read the values quickly enough...
from time import sleep
import serial
from ctypes import c_int16
class DatalinkDriver():
def __init__(self):
self.ser = serial.Serial ("/dev/ttyS0", 115200) #Open port with baud rate
self.telem_header = int.from_bytes(b'\x9b', 'big')
self.n_poles = 28
self.pole_pairs = self.n_poles/2
self.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(self, ix):
try:
keys = self.temp_table.keys()
if ix in keys:
return self.temp_table[ix]
x1 = min([val for val in keys if val > ix])
x0 = max([val for val in keys if val < ix])
y1 = self.temp_table[x1]
y0 = self.temp_table[x0]
yix = y0+ ((y1-y0)/(x1-x0)) * (ix-x0)
return yix
except:
return -1
class EscTelemetryMessage:
def __init__(self, data, pole_pairs, get_temp) -> 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, pole_pairs, get_temp) -> 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 = DatalinkDriver.EscTelemetryMessage(data = message_bytes[start_index:start_index+18], pole_pairs=pole_pairs, get_temp=get_temp)
self.esc_messages[esc_message.motor_ix-1] = esc_message
crc_sum = sum(message_bytes[7:158]) # FIXME
crc = int.from_bytes(message_bytes[158:], 'big') # FIXME
# print(f'{crc_sum=} {crc=}') # this is clearly still wrong....
def sample(self):
while True:
received_data = self.ser.read() #read serial port
sleep(0.05)
data_left = self.ser.in_waiting #check for number of remaining bytes
received_data += self.ser.read(data_left)
try:
dl_message = DatalinkDriver.TMotorDataLinkMessage(message_bytes=received_data, pole_pairs=self.pole_pairs, get_temp=self.get_temp)
except Exception as e:
print(e)
continue
msg = [None]*2
i=0
for esc_message in dl_message.esc_messages[:2]: #4
msg[i] = esc_message.__dict__
i += 1
return msg[0], msg[1]
Hello @holden-zenith, I would be extremely gratefull if you were able to do it (I'm sure there are others). It seems like using a lua script might be the easiest way to do it. Here is an example of a script that reads from the uart and wrights to the data flash logs, which in my opinion would be the ideal way to do it.
@holden-zenith, @eduardkieser,
Lua scripts are always welcome but I think this is core enough and would have enough users that an AP_ESC_Telem backend would be the way to go.
@khancyr I understood from your previous comments in the forum, that you would not be releasing your version.
I'm happy that I missunderstood you ! :)
Well I asked again T-Motor and they gave the permission to release code now!
This is great news. Once it's released, could we add a link to the documentation at the end of this thread? Or if there isnt going to be a dedicated section in the docs then at least a short how to here?
Hi @khancyr If you would like someone to review or test I'd be happy to help. We are all very eager to start testing this on our end.
I'm definitely interested as well for copter. There are lots of people out there using the Alpha's and this would be greatly appreciated. I'd be happy to test but am not a developer.
Peterbarker already made a good work on this, I am finishing the datalink integration and do some test. Probably next week into broad testing