53 lines
1.5 KiB
Python
53 lines
1.5 KiB
Python
import time
|
|
from threading import Thread
|
|
|
|
import serial
|
|
|
|
|
|
class CMPS14:
|
|
def __init__(self, com_port="/dev/cu.SLAB_USBtoUART", baudrate=115200) -> None:
|
|
"""
|
|
Other com port: /dev/cu.SLAB_USBtoUART
|
|
"""
|
|
self.serial = serial.Serial(com_port, baudrate=baudrate)
|
|
init_message = str(self.serial.readline())
|
|
assert 'I2C begin Failed' not in init_message, "No I2C sensor detected!"
|
|
self.heading = 0
|
|
self.heading_offset = 0
|
|
self.running = False
|
|
time.sleep(1)
|
|
|
|
def start(self):
|
|
t = Thread(target=self.update, args=())
|
|
t.daemon = True
|
|
self.running = True
|
|
t.start()
|
|
return self
|
|
|
|
def update(self):
|
|
while self.running:
|
|
try:
|
|
ser_bytes = str(self.serial.readline())
|
|
useful = ser_bytes.split('\\')[0].split('\'')[1]
|
|
heading = float(useful.split(':')[1])
|
|
self.heading = heading
|
|
except Exception as e:
|
|
print(f"Error: {e}")
|
|
|
|
def read(self):
|
|
corrected_heading = self.heading + self.heading_offset
|
|
if corrected_heading < 0:
|
|
corrected_heading += 360
|
|
if corrected_heading > 360:
|
|
corrected_heading -= 360
|
|
return corrected_heading
|
|
|
|
def set_heading_offset(self, offset):
|
|
self.heading_offset = offset
|
|
|
|
def update_heading_offset(self, offset):
|
|
self.heading_offset += offset
|
|
|
|
def stop(self):
|
|
self.running = False
|