113 lines
3.4 KiB
Python
113 lines
3.4 KiB
Python
import time
|
|
|
|
from esp32 import RMT
|
|
|
|
|
|
class Dshot:
|
|
def __init__(self, pin):
|
|
# https://brushlesswhoop.com/dshot-and-bidirectional-dshot/
|
|
# clock freq = 80 # Mhz
|
|
# clock divider = 7
|
|
# pulse time = 1 / (80 / 7) = 0.0875 us
|
|
# bit duration = 3.33 us
|
|
# T1H = 2.50 us
|
|
# T0H = 1.25 us
|
|
self._ON_PULSES = 57 # number of pulses for an ON bit
|
|
self._OFF_PULSES = 29 # number of pulses for an OFF bit
|
|
self._BIT_PULSES = 76 # total number of pulses per bit
|
|
self._PAUSE_PULSES = 21 # pause after sending packet
|
|
|
|
self.rmt = RMT(0, pin=pin, clock_div=7)
|
|
|
|
self._arm()
|
|
self._enable_telemetry()
|
|
|
|
def _arm(self, duration=2):
|
|
"""
|
|
Send arming sequence.
|
|
|
|
Args:
|
|
duration: How long to send the arming sequence, in seconds.
|
|
"""
|
|
t0 = time.time()
|
|
while time.time() - t0 < duration:
|
|
self.set_throttle(0)
|
|
# time.sleep(0.01)
|
|
|
|
def _enable_telemetry(self):
|
|
for i in range(6):
|
|
value = self._create_packet(33)
|
|
self._send(value)
|
|
for i in range(6):
|
|
value = self._create_packet(34)
|
|
self._send(value)
|
|
for i in range(6):
|
|
value = self._create_packet(35)
|
|
self._send(value)
|
|
for i in range(6):
|
|
value = self._create_packet(12)
|
|
self._send(value)
|
|
time.sleep(0.040)
|
|
|
|
# for i in range(10):
|
|
# self._send(self._create_packet(6))
|
|
# time.sleep(0.012)
|
|
|
|
# self._send(self._create_packet(42))
|
|
# self._send(self._create_packet(43))
|
|
# self._send(self._create_packet(44))
|
|
# self._send(self._create_packet(45))
|
|
# self._send(self._create_packet(46))
|
|
# self._send(self._create_packet(47))
|
|
|
|
# beep
|
|
# time.sleep(0.050)
|
|
# for i in range(5):
|
|
# self._send(self._create_packet(i + 1))
|
|
# time.sleep(0.250)
|
|
|
|
def _encode_throttle(self, value):
|
|
# 11 bit throttle: 2048 possible values.
|
|
# 0 is reserved for disarmed. 1-47 are reserved for special commands.
|
|
# Leaving 48 to 2047 (2000 steps) for the actual throttle value
|
|
value = min(max(value, 0.0), 1.0) # clamp to between 0 and 1
|
|
if value == 0:
|
|
value = 0 # disarmed
|
|
else:
|
|
value = int(value * 1999 + 48)
|
|
return value
|
|
|
|
def set_throttle(self, value: float, telemetry=True):
|
|
"""
|
|
Set throttle to a value between 0 and 1.
|
|
"""
|
|
value = self._encode_throttle(value)
|
|
value = self._create_packet(value)
|
|
self._send(value)
|
|
|
|
def _create_packet(self, value, telemetry=True):
|
|
# add telemetry bit
|
|
value = (value << 1) | telemetry
|
|
|
|
# add CRC (Cyclic Redundancy Check)
|
|
crc = (value ^ (value >> 4) ^ (value >> 8)) & 0x0F
|
|
value = (value << 4) | crc
|
|
|
|
return value
|
|
|
|
def _send(self, value):
|
|
"""
|
|
Send value to ESC.
|
|
"""
|
|
duration = []
|
|
|
|
for i in reversed(range(16)):
|
|
bit = (value & (2**i)) == 2**i # select bit
|
|
if bit == 1:
|
|
duration += [self._ON_PULSES, self._BIT_PULSES - self._ON_PULSES]
|
|
else:
|
|
duration += [self._OFF_PULSES, self._BIT_PULSES - self._OFF_PULSES]
|
|
|
|
duration[-1] += self._PAUSE_PULSES
|
|
|
|
self.rmt.write_pulses(duration, True)
|