mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-04 21:12:07 +08:00
sensord: move to system/ (#27531)
* sensord: move to system/ * add gitignore to releaes files old-commit-hash: 6f40f0d4427f4238c3939a64d1d6baa7d77f1ecc
This commit is contained in:
Adeeb Shihadeh
@@ -0,0 +1 @@
|
||||
_sensord
|
||||
@@ -0,0 +1,19 @@
|
||||
Import('env', 'arch', 'common', 'cereal', 'messaging')
|
||||
|
||||
sensors = [
|
||||
'sensors/file_sensor.cc',
|
||||
'sensors/i2c_sensor.cc',
|
||||
'sensors/light_sensor.cc',
|
||||
'sensors/bmx055_accel.cc',
|
||||
'sensors/bmx055_gyro.cc',
|
||||
'sensors/bmx055_magn.cc',
|
||||
'sensors/bmx055_temp.cc',
|
||||
'sensors/lsm6ds3_accel.cc',
|
||||
'sensors/lsm6ds3_gyro.cc',
|
||||
'sensors/lsm6ds3_temp.cc',
|
||||
'sensors/mmc5603nj_magn.cc',
|
||||
]
|
||||
libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread']
|
||||
if arch == "larch64":
|
||||
libs.append('i2c')
|
||||
env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
|
||||
Executable
+312
@@ -0,0 +1,312 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import time
|
||||
import signal
|
||||
import serial
|
||||
import struct
|
||||
import requests
|
||||
import urllib.parse
|
||||
from datetime import datetime
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from cereal import messaging
|
||||
from common.params import Params
|
||||
from system.swaglog import cloudlog
|
||||
from system.hardware import TICI
|
||||
from common.gpio import gpio_init, gpio_set
|
||||
from system.hardware.tici.pins import GPIO
|
||||
|
||||
UBLOX_TTY = "/dev/ttyHS0"
|
||||
|
||||
UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
|
||||
UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
|
||||
UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
|
||||
UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
|
||||
UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
|
||||
UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
|
||||
|
||||
def set_power(enabled: bool) -> None:
|
||||
gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_init(GPIO.UBLOX_PWR_EN, True)
|
||||
gpio_init(GPIO.UBLOX_RST_N, True)
|
||||
|
||||
gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, enabled)
|
||||
gpio_set(GPIO.UBLOX_RST_N, enabled)
|
||||
|
||||
def add_ubx_checksum(msg: bytes) -> bytes:
|
||||
A = B = 0
|
||||
for b in msg[2:]:
|
||||
A = (A + b) % 256
|
||||
B = (B + A) % 256
|
||||
return msg + bytes([A, B])
|
||||
|
||||
def get_assistnow_messages(token: bytes) -> List[bytes]:
|
||||
# make request
|
||||
# TODO: implement adding the last known location
|
||||
r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
|
||||
'token': token,
|
||||
'gnss': 'gps,glo',
|
||||
'datatype': 'eph,alm,aux',
|
||||
}, safe=':,'), timeout=5)
|
||||
assert r.status_code == 200, "Got invalid status code"
|
||||
dat = r.content
|
||||
|
||||
# split up messages
|
||||
msgs = []
|
||||
while len(dat) > 0:
|
||||
assert dat[:2] == b"\xB5\x62"
|
||||
msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
|
||||
msgs.append(dat[:msg_len])
|
||||
dat = dat[msg_len:]
|
||||
return msgs
|
||||
|
||||
|
||||
class TTYPigeon():
|
||||
def __init__(self):
|
||||
self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
|
||||
|
||||
def send(self, dat: bytes) -> None:
|
||||
self.tty.write(dat)
|
||||
|
||||
def receive(self) -> bytes:
|
||||
dat = b''
|
||||
while len(dat) < 0x1000:
|
||||
d = self.tty.read(0x40)
|
||||
dat += d
|
||||
if len(d) == 0:
|
||||
break
|
||||
return dat
|
||||
|
||||
def set_baud(self, baud: int) -> None:
|
||||
self.tty.baudrate = baud
|
||||
|
||||
def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool:
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
if ack in dat:
|
||||
cloudlog.debug("Received ACK from ublox")
|
||||
return True
|
||||
elif nack in dat:
|
||||
cloudlog.error("Received NACK from ublox")
|
||||
return False
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None:
|
||||
self.send(dat)
|
||||
self.wait_for_ack(ack, nack)
|
||||
|
||||
def wait_for_backup_restore_status(self, timeout: float = 1.) -> int:
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
|
||||
if position >= 0 and len(dat) >= position + 11:
|
||||
return dat[position + 10]
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No backup restore response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
def reset_device(self) -> bool:
|
||||
# deleting the backup does not always work on first try (mostly on second try)
|
||||
for _ in range(5):
|
||||
# device cold start
|
||||
self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d")
|
||||
time.sleep(1) # wait for cold start
|
||||
init_baudrate(self)
|
||||
|
||||
# clear configuration
|
||||
self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7")
|
||||
|
||||
# clear flash memory (almanac backup)
|
||||
self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0")
|
||||
|
||||
# try restoring backup to verify it got deleted
|
||||
self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
|
||||
# 1: failed to restore, 2: could restore, 3: no backup
|
||||
status = self.wait_for_backup_restore_status()
|
||||
if status == 1 or status == 3:
|
||||
return True
|
||||
return False
|
||||
|
||||
def init_baudrate(pigeon: TTYPigeon):
|
||||
# ublox default setting on startup is 9600 baudrate
|
||||
pigeon.set_baud(9600)
|
||||
|
||||
# $PUBX,41,1,0007,0003,460800,0*15\r\n
|
||||
pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A")
|
||||
time.sleep(0.1)
|
||||
pigeon.set_baud(460800)
|
||||
|
||||
|
||||
def initialize_pigeon(pigeon: TTYPigeon) -> bool:
|
||||
# try initializing a few times
|
||||
for _ in range(10):
|
||||
try:
|
||||
|
||||
# setup port config
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24")
|
||||
|
||||
# UBX-CFG-RATE (0x06 0x08)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
|
||||
|
||||
# UBX-CFG-NAV5 (0x06 0x24)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
|
||||
|
||||
# UBX-CFG-ODO (0x06 0x1E)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
|
||||
|
||||
# UBX-CFG-NAV5 (0x06 0x24)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
|
||||
|
||||
# UBX-CFG-MSG (set message rate)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
|
||||
cloudlog.debug("pigeon configured")
|
||||
|
||||
# try restoring almanac backup
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
|
||||
restore_status = pigeon.wait_for_backup_restore_status()
|
||||
if restore_status == 2:
|
||||
cloudlog.warning("almanac backup restored")
|
||||
elif restore_status == 3:
|
||||
cloudlog.warning("no almanac backup found")
|
||||
else:
|
||||
cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
|
||||
|
||||
# sending time to ublox
|
||||
t_now = datetime.utcnow()
|
||||
if t_now >= datetime(2021, 6, 1):
|
||||
cloudlog.warning("Sending current time to ublox")
|
||||
|
||||
# UBX-MGA-INI-TIME_UTC
|
||||
msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
|
||||
0x10,
|
||||
0x00,
|
||||
0x00,
|
||||
0x80,
|
||||
t_now.year,
|
||||
t_now.month,
|
||||
t_now.day,
|
||||
t_now.hour,
|
||||
t_now.minute,
|
||||
t_now.second,
|
||||
0,
|
||||
30,
|
||||
0
|
||||
))
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
|
||||
# try getting AssistNow if we have a token
|
||||
token = Params().get('AssistNowToken')
|
||||
if token is not None:
|
||||
try:
|
||||
for msg in get_assistnow_messages(token):
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
cloudlog.warning("AssistNow messages sent")
|
||||
except Exception:
|
||||
cloudlog.warning("failed to get AssistNow messages")
|
||||
|
||||
cloudlog.warning("Pigeon GPS on!")
|
||||
break
|
||||
except TimeoutError:
|
||||
cloudlog.warning("Initialization failed, trying again!")
|
||||
else:
|
||||
cloudlog.warning("Failed to initialize pigeon")
|
||||
return False
|
||||
return True
|
||||
|
||||
def deinitialize_and_exit(pigeon: Optional[TTYPigeon]):
|
||||
cloudlog.warning("Storing almanac in ublox flash")
|
||||
|
||||
if pigeon is not None:
|
||||
# controlled GNSS stop
|
||||
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
|
||||
|
||||
# store almanac in flash
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
|
||||
try:
|
||||
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
|
||||
cloudlog.warning("Done storing almanac")
|
||||
else:
|
||||
cloudlog.error("Error storing almanac")
|
||||
except TimeoutError:
|
||||
pass
|
||||
|
||||
# turn off power and exit cleanly
|
||||
set_power(False)
|
||||
sys.exit(0)
|
||||
|
||||
def create_pigeon() -> Tuple[TTYPigeon, messaging.PubMaster]:
|
||||
pigeon = None
|
||||
|
||||
# register exit handler
|
||||
signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
|
||||
pm = messaging.PubMaster(['ubloxRaw'])
|
||||
|
||||
# power cycle ublox
|
||||
set_power(False)
|
||||
time.sleep(0.1)
|
||||
set_power(True)
|
||||
time.sleep(0.5)
|
||||
|
||||
pigeon = TTYPigeon()
|
||||
return pigeon, pm
|
||||
|
||||
def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0):
|
||||
|
||||
start_time = time.monotonic()
|
||||
def end_condition():
|
||||
return True if duration == 0 else time.monotonic() - start_time < duration
|
||||
|
||||
while end_condition():
|
||||
dat = pigeon.receive()
|
||||
if len(dat) > 0:
|
||||
if dat[0] == 0x00:
|
||||
cloudlog.warning("received invalid data from ublox, re-initing!")
|
||||
init_baudrate(pigeon)
|
||||
initialize_pigeon(pigeon)
|
||||
continue
|
||||
|
||||
# send out to socket
|
||||
msg = messaging.new_message('ubloxRaw', len(dat))
|
||||
msg.ubloxRaw = dat[:]
|
||||
pm.send('ubloxRaw', msg)
|
||||
else:
|
||||
# prevent locking up a CPU core if ublox disconnects
|
||||
time.sleep(0.001)
|
||||
|
||||
|
||||
def main():
|
||||
assert TICI, "unsupported hardware for pigeond"
|
||||
|
||||
pigeon, pm = create_pigeon()
|
||||
init_baudrate(pigeon)
|
||||
initialize_pigeon(pigeon)
|
||||
|
||||
# start receiving data
|
||||
run_receiving(pigeon, pm)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+66
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python3
|
||||
import cereal.messaging as messaging
|
||||
from laika import constants
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
|
||||
|
||||
meas = None
|
||||
while 1:
|
||||
sm.update()
|
||||
if sm['ubloxGnss'].which() == "measurementReport":
|
||||
meas = sm['ubloxGnss'].measurementReport.measurements
|
||||
if not sm.updated['qcomGnss'] or meas is None:
|
||||
continue
|
||||
report = sm['qcomGnss'].measurementReport
|
||||
if report.source not in [0, 1]:
|
||||
continue
|
||||
GLONASS = report.source == 1
|
||||
recv_time = report.milliseconds / 1000
|
||||
|
||||
car = []
|
||||
print("qcom has ", list(sorted([x.svId for x in report.sv])))
|
||||
print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)])))
|
||||
for i in report.sv:
|
||||
# match to ublox
|
||||
tm = None
|
||||
for m in meas:
|
||||
if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
|
||||
tm = m
|
||||
if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
|
||||
tm = m
|
||||
if tm is None:
|
||||
continue
|
||||
|
||||
if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
|
||||
sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
|
||||
ublox_psuedorange = tm.pseudorange
|
||||
qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
|
||||
if GLONASS:
|
||||
glonass_freq = tm.glonassFrequencyIndex - 7
|
||||
ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
|
||||
else:
|
||||
ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
|
||||
qcom_speed = i.unfilteredSpeed
|
||||
car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
|
||||
|
||||
if len(car) == 0:
|
||||
print("nothing to compare")
|
||||
continue
|
||||
|
||||
pr_err, speed_err = 0., 0.
|
||||
for c in car:
|
||||
ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
|
||||
pr_err += ublox_psuedorange - qcom_psuedorange
|
||||
speed_err += ublox_speed - qcom_speed
|
||||
pr_err /= len(car)
|
||||
speed_err /= len(car)
|
||||
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
|
||||
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)): # type: ignore
|
||||
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
|
||||
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" %
|
||||
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
|
||||
ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
import select
|
||||
from serial import Serial
|
||||
from crcmod import mkCrcFun
|
||||
from struct import pack, unpack_from, calcsize
|
||||
|
||||
class ModemDiag:
|
||||
def __init__(self):
|
||||
self.serial = self.open_serial()
|
||||
self.pend = b''
|
||||
|
||||
def open_serial(self):
|
||||
serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
|
||||
serial.flush()
|
||||
serial.reset_input_buffer()
|
||||
serial.reset_output_buffer()
|
||||
return serial
|
||||
|
||||
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
|
||||
ESCAPE_CHAR = b'\x7d'
|
||||
TRAILER_CHAR = b'\x7e'
|
||||
|
||||
def hdlc_encapsulate(self, payload):
|
||||
payload += pack('<H', ModemDiag.ccitt_crc16(payload))
|
||||
payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
|
||||
payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
|
||||
payload += self.TRAILER_CHAR
|
||||
return payload
|
||||
|
||||
def hdlc_decapsulate(self, payload):
|
||||
assert len(payload) >= 3
|
||||
assert payload[-1:] == self.TRAILER_CHAR
|
||||
payload = payload[:-1]
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
|
||||
assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
|
||||
return payload[:-2]
|
||||
|
||||
def recv(self):
|
||||
# self.serial.read_until makes tons of syscalls!
|
||||
raw_payload = [self.pend]
|
||||
while self.TRAILER_CHAR not in raw_payload[-1]:
|
||||
select.select([self.serial.fd], [], [])
|
||||
raw = self.serial.read(0x10000)
|
||||
raw_payload.append(raw)
|
||||
raw_payload = b''.join(raw_payload)
|
||||
raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
|
||||
raw_payload += self.TRAILER_CHAR
|
||||
unframed_message = self.hdlc_decapsulate(raw_payload)
|
||||
return unframed_message[0], unframed_message[1:]
|
||||
|
||||
def send(self, packet_type, packet_payload):
|
||||
self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
|
||||
|
||||
# *** end class ***
|
||||
|
||||
DIAG_LOG_F = 16
|
||||
DIAG_LOG_CONFIG_F = 115
|
||||
LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
|
||||
LOG_CONFIG_SET_MASK_OP = 3
|
||||
LOG_CONFIG_SUCCESS_S = 0
|
||||
|
||||
def send_recv(diag, packet_type, packet_payload):
|
||||
diag.send(packet_type, packet_payload)
|
||||
while 1:
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
break
|
||||
return opcode, payload
|
||||
|
||||
def setup_logs(diag, types_to_log):
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
|
||||
|
||||
header_spec = '<3xII'
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
|
||||
log_masks = unpack_from('<16I', payload, calcsize(header_spec))
|
||||
|
||||
for log_type, log_mask_bitsize in enumerate(log_masks):
|
||||
if log_mask_bitsize:
|
||||
log_mask = [0] * ((log_mask_bitsize+7)//8)
|
||||
for i in range(log_mask_bitsize):
|
||||
if ((log_type<<12)|i) in types_to_log:
|
||||
log_mask[i//8] |= 1 << (i%8)
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
|
||||
LOG_CONFIG_SET_MASK_OP,
|
||||
log_type,
|
||||
log_mask_bitsize
|
||||
) + bytes(log_mask))
|
||||
assert opcode == DIAG_LOG_CONFIG_F
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_SET_MASK_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
Executable
+366
@@ -0,0 +1,366 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import signal
|
||||
import itertools
|
||||
import math
|
||||
import time
|
||||
import subprocess
|
||||
from typing import NoReturn
|
||||
from struct import unpack_from, calcsize, pack
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from common.gpio import gpio_init, gpio_set
|
||||
from laika.gps_time import GPSTime
|
||||
from system.hardware.tici.pins import GPIO
|
||||
from system.swaglog import cloudlog
|
||||
from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
|
||||
from system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
|
||||
gps_measurement_report, gps_measurement_report_sv,
|
||||
glonass_measurement_report, glonass_measurement_report_sv,
|
||||
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
|
||||
|
||||
DEBUG = int(os.getenv("DEBUG", "0"))==1
|
||||
|
||||
LOG_TYPES = [
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
|
||||
]
|
||||
|
||||
|
||||
miscStatusFields = {
|
||||
"multipathEstimateIsValid": 0,
|
||||
"directionIsValid": 1,
|
||||
}
|
||||
|
||||
measurementStatusFields = {
|
||||
"subMillisecondIsValid": 0,
|
||||
"subBitTimeIsKnown": 1,
|
||||
"satelliteTimeIsKnown": 2,
|
||||
"bitEdgeConfirmedFromSignal": 3,
|
||||
"measuredVelocity": 4,
|
||||
"fineOrCoarseVelocity": 5,
|
||||
"lockPointValid": 6,
|
||||
"lockPointPositive": 7,
|
||||
|
||||
"lastUpdateFromDifference": 9,
|
||||
"lastUpdateFromVelocityDifference": 10,
|
||||
"strongIndicationOfCrossCorelation": 11,
|
||||
"tentativeMeasurement": 12,
|
||||
"measurementNotUsable": 13,
|
||||
"sirCheckIsNeeded": 14,
|
||||
"probationMode": 15,
|
||||
|
||||
"multipathIndicator": 24,
|
||||
"imdJammingIndicator": 25,
|
||||
"lteB13TxJammingIndicator": 26,
|
||||
"freshMeasurementIndicator": 27,
|
||||
}
|
||||
|
||||
measurementStatusGPSFields = {
|
||||
"gpsRoundRobinRxDiversity": 18,
|
||||
"gpsRxDiversity": 19,
|
||||
"gpsLowBandwidthRxDiversityCombined": 20,
|
||||
"gpsHighBandwidthNu4": 21,
|
||||
"gpsHighBandwidthNu8": 22,
|
||||
"gpsHighBandwidthUniform": 23,
|
||||
}
|
||||
|
||||
measurementStatusGlonassFields = {
|
||||
"glonassMeanderBitEdgeValid": 16,
|
||||
"glonassTimeMarkValid": 17
|
||||
}
|
||||
|
||||
|
||||
def try_setup_logs(diag, log_types):
|
||||
for _ in range(5):
|
||||
try:
|
||||
setup_logs(diag, log_types)
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("setup logs failed, trying again")
|
||||
else:
|
||||
raise Exception(f"setup logs failed, {log_types=}")
|
||||
|
||||
def at_cmd(cmd: str) -> None:
|
||||
for _ in range(5):
|
||||
try:
|
||||
subprocess.check_call(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True)
|
||||
break
|
||||
except subprocess.CalledProcessError:
|
||||
cloudlog.exception("rawgps.mmcli_command_failed")
|
||||
else:
|
||||
raise Exception(f"failed to execute mmcli command {cmd=}")
|
||||
|
||||
|
||||
def gps_enabled() -> bool:
|
||||
try:
|
||||
p = subprocess.check_output("mmcli -m any --command=\"AT+QGPS?\"", shell=True)
|
||||
return b"QGPS: 1" in p
|
||||
except subprocess.CalledProcessError as exc:
|
||||
raise Exception("failed to execute QGPS mmcli command") from exc
|
||||
|
||||
def setup_quectel(diag: ModemDiag):
|
||||
# enable OEMDRE in the NV
|
||||
# TODO: it has to reboot for this to take effect
|
||||
DIAG_NV_READ_F = 38
|
||||
DIAG_NV_WRITE_F = 39
|
||||
NV_GNSS_OEM_FEATURE_MASK = 7165
|
||||
send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
|
||||
send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
|
||||
|
||||
setup_logs(diag, LOG_TYPES)
|
||||
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
|
||||
# disable DPO power savings for more accuracy
|
||||
at_cmd("AT+QGPSCFG=\"dpoenable\",0")
|
||||
# don't automatically turn on GNSS on powerup
|
||||
at_cmd("AT+QGPSCFG=\"autogps\",0")
|
||||
|
||||
at_cmd("AT+QGPSSUPLURL=\"supl.google.com:7275\"")
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
|
||||
at_cmd("AT+QGPS=1")
|
||||
|
||||
# enable OEMDRE mode
|
||||
DIAG_SUBSYS_CMD_F = 75
|
||||
DIAG_SUBSYS_GPS = 13
|
||||
CGPS_DIAG_PDAPI_CMD = 0x64
|
||||
CGPS_OEM_CONTROL = 202
|
||||
GPSDIAG_OEMFEATURE_DRE = 1
|
||||
GPSDIAG_OEM_DRE_ON = 1
|
||||
|
||||
# gpsdiag_OemControlReqType
|
||||
send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
|
||||
DIAG_SUBSYS_GPS, # Subsystem Id
|
||||
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
|
||||
CGPS_OEM_CONTROL, # CGPS Command Code
|
||||
0, # Version
|
||||
GPSDIAG_OEMFEATURE_DRE,
|
||||
GPSDIAG_OEM_DRE_ON,
|
||||
0,0
|
||||
))
|
||||
|
||||
def teardown_quectel(diag):
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
try_setup_logs(diag, [])
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
|
||||
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
|
||||
|
||||
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
|
||||
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
|
||||
|
||||
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
|
||||
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
|
||||
|
||||
unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
# wait for ModemManager to come up
|
||||
cloudlog.warning("waiting for modem to come up")
|
||||
while True:
|
||||
ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
|
||||
if ret == 0:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
|
||||
# connect to modem
|
||||
diag = ModemDiag()
|
||||
|
||||
def cleanup(sig, frame):
|
||||
cloudlog.warning(f"caught sig {sig}, disabling quectel gps")
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, False)
|
||||
teardown_quectel(diag)
|
||||
cloudlog.warning("quectel cleanup done")
|
||||
sys.exit(0)
|
||||
signal.signal(signal.SIGINT, cleanup)
|
||||
signal.signal(signal.SIGTERM, cleanup)
|
||||
|
||||
setup_quectel(diag)
|
||||
cloudlog.warning("quectel setup done")
|
||||
gpio_init(GPIO.UBLOX_PWR_EN, True)
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, True)
|
||||
|
||||
pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
|
||||
|
||||
while 1:
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
cloudlog.error(f"Unhandled opcode: {opcode}")
|
||||
continue
|
||||
|
||||
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
|
||||
if pending_msgs > 0:
|
||||
cloudlog.debug("have %d pending messages" % pending_msgs)
|
||||
assert log_outer_length == len(inner_log_packet)
|
||||
|
||||
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
|
||||
assert log_inner_length == len(inner_log_packet)
|
||||
|
||||
if log_type not in LOG_TYPES:
|
||||
continue
|
||||
|
||||
if DEBUG:
|
||||
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
|
||||
|
||||
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drMeasurementReport')
|
||||
report = gnss.drMeasurementReport
|
||||
|
||||
dat = unpack_oemdre_meas(log_payload)
|
||||
for k,v in dat.items():
|
||||
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
|
||||
k += "Ms"
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "svCount" or k.startswith("cdmaClockInfo["):
|
||||
# TODO: should we save cdmaClockInfo?
|
||||
pass
|
||||
elif k == "systemRtcValid":
|
||||
setattr(report, k, bool(v))
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
|
||||
report.init('sv', dat['svCount'])
|
||||
sats = log_payload[size_oemdre_meas:]
|
||||
for i in range(dat['svCount']):
|
||||
sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
for k,v in sat.items():
|
||||
if k in ["unkn", "measurementStatus2"]:
|
||||
pass
|
||||
elif k == "multipathEstimateValid":
|
||||
sv.measurementStatus.multipathEstimateIsValid = bool(v)
|
||||
elif k == "directionValid":
|
||||
sv.measurementStatus.directionIsValid = bool(v)
|
||||
elif k == "goodParity":
|
||||
setattr(sv, k, bool(v))
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in measurementStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
pm.send('qcomGnss', msg)
|
||||
elif log_type == LOG_GNSS_POSITION_REPORT:
|
||||
report = unpack_position(log_payload)
|
||||
if report["u_PosSource"] != 2:
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
msg = messaging.new_message('gpsLocation')
|
||||
gps = msg.gpsLocation
|
||||
gps.flags = 1
|
||||
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
|
||||
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
|
||||
gps.altitude = report["q_FltFinalPosAlt"]
|
||||
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
|
||||
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
|
||||
gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
|
||||
1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
|
||||
gps.source = log.GpsLocationData.SensorSource.qcomdiag
|
||||
gps.vNED = vNED
|
||||
gps.verticalAccuracy = report["q_FltVdop"]
|
||||
gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi
|
||||
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
|
||||
|
||||
pm.send('gpsLocation', msg)
|
||||
|
||||
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
dat = unpack_svpoly(log_payload)
|
||||
dat = relist(dat)
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drSvPoly')
|
||||
poly = gnss.drSvPoly
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "flags":
|
||||
pass
|
||||
else:
|
||||
setattr(poly, k, v)
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('measurementReport')
|
||||
report = gnss.measurementReport
|
||||
|
||||
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
|
||||
dat = unpack_gps_meas(log_payload)
|
||||
sats = log_payload[size_gps_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
|
||||
report.source = 0 # gps
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
|
||||
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
|
||||
dat = unpack_glonass_meas(log_payload)
|
||||
sats = log_payload[size_glonass_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
|
||||
report.source = 1 # glonass
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
|
||||
else:
|
||||
assert False
|
||||
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 0
|
||||
elif k == "week":
|
||||
report.gpsWeek = v
|
||||
elif k == "svCount":
|
||||
pass
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
report.init('sv', dat['svCount'])
|
||||
if dat['svCount'] > 0:
|
||||
assert len(sats)//dat['svCount'] == size_meas_sv
|
||||
for i in range(dat['svCount']):
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
|
||||
for k,v in sat.items():
|
||||
if k == "parityErrorCount":
|
||||
sv.gpsParityErrorCount = v
|
||||
elif k == "frequencyIndex":
|
||||
sv.glonassFrequencyIndex = v
|
||||
elif k == "hemmingErrorCount":
|
||||
sv.glonassHemmingErrorCount = v
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in itertools.chain(*measurement_status_fields):
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "miscStatus":
|
||||
for kk,vv in miscStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "pad":
|
||||
pass
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,354 @@
|
||||
from struct import unpack_from, calcsize
|
||||
|
||||
LOG_GNSS_POSITION_REPORT = 0x1476
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
|
||||
LOG_GNSS_CLOCK_REPORT = 0x1478
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
|
||||
LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
|
||||
LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
|
||||
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
|
||||
|
||||
LOG_GNSS_ME_DPO_STATUS = 0x1838
|
||||
LOG_GNSS_CD_DB_REPORT = 0x147B
|
||||
LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
|
||||
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
|
||||
LOG_GNSS_CONFIGURATION_STATE = 0x1516
|
||||
|
||||
oemdre_measurement_report = """
|
||||
uint8_t version;
|
||||
uint8_t reason;
|
||||
uint8_t sv_count;
|
||||
uint8_t seq_num;
|
||||
uint8_t seq_max;
|
||||
uint16_t rf_loss;
|
||||
|
||||
uint8_t system_rtc_valid;
|
||||
uint32_t f_count;
|
||||
uint32_t clock_resets;
|
||||
uint64_t system_rtc_time;
|
||||
|
||||
uint8_t gps_leap_seconds;
|
||||
uint8_t gps_leap_seconds_uncertainty;
|
||||
float gps_to_glonass_time_bias_milliseconds;
|
||||
float gps_to_glonass_time_bias_milliseconds_uncertainty;
|
||||
|
||||
uint16_t gps_week;
|
||||
uint32_t gps_milliseconds;
|
||||
uint32_t gps_time_bias;
|
||||
uint32_t gps_clock_time_uncertainty;
|
||||
uint8_t gps_clock_source;
|
||||
|
||||
uint8_t glonass_clock_source;
|
||||
uint8_t glonass_year;
|
||||
uint16_t glonass_day;
|
||||
uint32_t glonass_milliseconds;
|
||||
float glonass_time_bias;
|
||||
float glonass_clock_time_uncertainty;
|
||||
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t frequency_source;
|
||||
|
||||
uint32_t cdma_clock_info[5];
|
||||
|
||||
uint8_t source;
|
||||
"""
|
||||
|
||||
oemdre_svpoly_report = """
|
||||
uint8_t version;
|
||||
uint16_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t flags;
|
||||
uint16_t iode;
|
||||
double t0;
|
||||
double xyz0[3];
|
||||
double xyzN[9];
|
||||
float other[4];
|
||||
float position_uncertainty;
|
||||
float iono_delay;
|
||||
float iono_dot;
|
||||
float sbas_iono_delay;
|
||||
float sbas_iono_dot;
|
||||
float tropo_delay;
|
||||
float elevation;
|
||||
float elevation_dot;
|
||||
float elevation_uncertainty;
|
||||
double velocity_coeff[12];
|
||||
"""
|
||||
|
||||
|
||||
oemdre_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
uint8_t unkn;
|
||||
int8_t glonass_frequency_index;
|
||||
uint32_t observation_state;
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t filter_stages;
|
||||
uint8_t predetect_interval;
|
||||
uint8_t cycle_slip_count;
|
||||
uint16_t postdetections;
|
||||
|
||||
uint32_t measurement_status;
|
||||
uint32_t measurement_status2;
|
||||
|
||||
uint16_t carrier_noise;
|
||||
uint16_t rf_loss;
|
||||
int16_t latency;
|
||||
|
||||
float filtered_measurement_fraction;
|
||||
uint32_t filtered_measurement_integral;
|
||||
float filtered_time_uncertainty;
|
||||
float filtered_speed;
|
||||
float filtered_speed_uncertainty;
|
||||
|
||||
float unfiltered_measurement_fraction;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
|
||||
uint8_t multipath_estimate_valid;
|
||||
uint32_t multipath_estimate;
|
||||
uint8_t direction_valid;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
float doppler_acceleration;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
|
||||
uint64_t carrier_phase;
|
||||
uint32_t f_count;
|
||||
|
||||
uint16_t parity_error_count;
|
||||
uint8_t good_parity;
|
||||
"""
|
||||
|
||||
glonass_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint8_t glonass_cycle_number;
|
||||
uint16_t glonass_number_of_days;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
glonass_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t observation_state; // SVObservationStates
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t hemming_error_count;
|
||||
uint8_t filter_stages;
|
||||
uint16_t carrier_noise;
|
||||
int16_t latency;
|
||||
uint8_t predetect_interval;
|
||||
uint16_t postdetections;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_measurement_fraction;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
uint8_t cycle_slip_count;
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
gps_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint16_t week;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
gps_measurement_report_sv = """
|
||||
uint8_t sv_id; // SV PRN
|
||||
uint8_t observation_state; // SV Observation state
|
||||
uint8_t observations; // Count of all observation (both success and failure)
|
||||
uint8_t good_observations; // Count of Good observations
|
||||
uint16_t parity_error_count; // Carrier to Code filtering N count
|
||||
uint8_t filter_stages; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t carrier_noise; // CNo. Units of 0.1 dB
|
||||
int16_t latency; // Age of the measurement in msecs (+ve meas Meas precedes ref time)
|
||||
uint8_t predetect_interval; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t postdetections; // Num Post-Detections (uints of PreInts
|
||||
uint32_t unfiltered_measurement_integral; // Range of 0 thru (WEEK_MSECS-1) [msecs]
|
||||
float unfiltered_measurement_fraction; // Range of 0 thru 0.99999 [msecs]
|
||||
float unfiltered_time_uncertainty; // Time uncertainty (msec)
|
||||
float unfiltered_speed; // Speed estimate (meters/sec)
|
||||
float unfiltered_speed_uncertainty; // Speed uncertainty estimate (meters/sec)
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth; // Azimuth (radians)
|
||||
float elevation; // Elevation (radians)
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed; // Carrier phase derived speed
|
||||
float fine_speed_uncertainty; // Carrier phase derived speed UNC
|
||||
uint8_t cycle_slip_count; // Increments when a CSlip is detected
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
position_report = """
|
||||
uint8 u_Version; /* Version number of DM log */
|
||||
uint32 q_Fcount; /* Local millisecond counter */
|
||||
uint8 u_PosSource; /* Source of position information */ /* 0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database */
|
||||
uint32 q_Reserved1; /* Reserved memory field */
|
||||
uint16 w_PosVelFlag; /* Position velocity bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 q_PosVelFlag2; /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
|
||||
uint8 u_FailureCode; /* Failure code: (see DM log 0x1476 documentation) */
|
||||
uint16 w_FixEvents; /* Fix events bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 _fake_align_week_number;
|
||||
uint16 w_GpsWeekNumber; /* GPS week number of position */
|
||||
uint32 q_GpsFixTimeMs; /* GPS fix time of week of in milliseconds */
|
||||
uint8 u_GloNumFourYear; /* Number of Glonass four year cycles */
|
||||
uint16 w_GloNumDaysInFourYear; /* Glonass calendar day in four year cycle */
|
||||
uint32 q_GloFixTimeMs; /* Glonass fix time of day in milliseconds */
|
||||
uint32 q_PosCount; /* Integer count of the number of unique positions reported */
|
||||
uint64 t_DblFinalPosLatLon[2]; /* Final latitude and longitude of position in radians */
|
||||
uint32 q_FltFinalPosAlt; /* Final height-above-ellipsoid altitude of position */
|
||||
uint32 q_FltHeadingRad; /* User heading in radians */
|
||||
uint32 q_FltHeadingUncRad; /* User heading uncertainty in radians */
|
||||
uint32 q_FltVelEnuMps[3]; /* User velocity in east, north, up coordinate frame. In meters per second. */
|
||||
uint32 q_FltVelSigmaMps[3]; /* Gaussian 1-sigma value for east, north, up components of user velocity */
|
||||
uint32 q_FltClockBiasMeters; /* Receiver clock bias in meters */
|
||||
uint32 q_FltClockBiasSigmaMeters; /* Gaussian 1-sigma value for receiver clock bias in meters */
|
||||
uint32 q_FltGGTBMeters; /* GPS to Glonass time bias in meters */
|
||||
uint32 q_FltGGTBSigmaMeters; /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltGBTBMeters; /* GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltGBTBSigmaMeters; /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltBGTBMeters; /* BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltBGTBSigmaMeters; /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGGTBMeters; /* Filtered GPS to Glonass time bias in meters */
|
||||
uint32 q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGBTBMeters; /* Filtered GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltFiltBGTBMeters; /* Filtered BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltSftOffsetSec; /* SFT offset as computed by WLS in seconds */
|
||||
uint32 q_FltSftOffsetSigmaSec; /* Gaussian 1-sigma value for SFT offset in seconds */
|
||||
uint32 q_FltClockDriftMps; /* Clock drift (clock frequency bias) in meters per second */
|
||||
uint32 q_FltClockDriftSigmaMps; /* Gaussian 1-sigma value for clock drift in meters per second */
|
||||
uint32 q_FltFilteredAlt; /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltFilteredAltSigma; /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
|
||||
uint32 q_FltRawAlt; /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltRawAltSigma; /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
|
||||
uint32 align_Flt[14];
|
||||
uint32 q_FltPdop; /* 3D position dilution of precision as computed from the unweighted
|
||||
uint32 q_FltHdop; /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint32 q_FltVdop; /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint8 u_EllipseConfidence; /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
|
||||
uint32 q_FltEllipseAngle; /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
|
||||
uint32 q_FltEllipseSemimajorAxis; /* Semimajor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltEllipseSemiminorAxis; /* Semiminor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltPosSigmaVertical; /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
|
||||
uint8 u_HorizontalReliability; /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High */
|
||||
uint8 u_VerticalReliability; /* Vertical position reliability */
|
||||
uint16 w_Reserved2; /* Reserved memory field */
|
||||
uint32 q_FltGnssHeadingRad; /* User heading in radians derived from GNSS only solution */
|
||||
uint32 q_FltGnssHeadingUncRad; /* User heading uncertainty in radians derived from GNSS only solution */
|
||||
uint32 q_SensorDataUsageMask; /* Denotes which additional sensor data were used to compute this position fix. BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
|
||||
uint32 q_SensorAidMask; /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
|
||||
uint8 u_NumGpsSvsUsed; /* The number of GPS SVs used in the fix */
|
||||
uint8 u_TotalGpsSvs; /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumGloSvsUsed; /* The number of Glonass SVs used in the fix */
|
||||
uint8 u_TotalGloSvs; /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumBdsSvsUsed; /* The number of BeiDou SVs used in the fix */
|
||||
uint8 u_TotalBdsSvs; /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
|
||||
"""
|
||||
|
||||
def name_to_camelcase(nam):
|
||||
ret = []
|
||||
i = 0
|
||||
while i < len(nam):
|
||||
if nam[i] == "_":
|
||||
ret.append(nam[i+1].upper())
|
||||
i += 2
|
||||
else:
|
||||
ret.append(nam[i])
|
||||
i += 1
|
||||
return ''.join(ret)
|
||||
|
||||
def parse_struct(ss):
|
||||
st = "<"
|
||||
nams = []
|
||||
for l in ss.strip().split("\n"):
|
||||
if len(l.strip()) == 0:
|
||||
continue
|
||||
typ, nam = l.split(";")[0].split()
|
||||
#print(typ, nam)
|
||||
if typ == "float" or '_Flt' in nam:
|
||||
st += "f"
|
||||
elif typ == "double" or '_Dbl' in nam:
|
||||
st += "d"
|
||||
elif typ in ["uint8", "uint8_t"]:
|
||||
st += "B"
|
||||
elif typ in ["int8", "int8_t"]:
|
||||
st += "b"
|
||||
elif typ in ["uint32", "uint32_t"]:
|
||||
st += "I"
|
||||
elif typ in ["int32", "int32_t"]:
|
||||
st += "i"
|
||||
elif typ in ["uint16", "uint16_t"]:
|
||||
st += "H"
|
||||
elif typ in ["int16", "int16_t"]:
|
||||
st += "h"
|
||||
elif typ in ["uint64", "uint64_t"]:
|
||||
st += "Q"
|
||||
else:
|
||||
print("unknown type", typ)
|
||||
assert False
|
||||
if '[' in nam:
|
||||
cnt = int(nam.split("[")[1].split("]")[0])
|
||||
st += st[-1]*(cnt-1)
|
||||
for i in range(cnt):
|
||||
nams.append("%s[%d]" % (nam.split("[")[0], i))
|
||||
else:
|
||||
nams.append(nam)
|
||||
return st, nams
|
||||
|
||||
def dict_unpacker(ss, camelcase = False):
|
||||
st, nams = parse_struct(ss)
|
||||
if camelcase:
|
||||
nams = [name_to_camelcase(x) for x in nams]
|
||||
sz = calcsize(st)
|
||||
return lambda x: dict(zip(nams, unpack_from(st, x))), sz
|
||||
|
||||
def relist(dat):
|
||||
list_keys = set()
|
||||
for key in dat.keys():
|
||||
if '[' in key:
|
||||
list_keys.add(key.split('[')[0])
|
||||
list_dict = {}
|
||||
for list_key in list_keys:
|
||||
list_dict[list_key] = []
|
||||
i = 0
|
||||
while True:
|
||||
key = list_key + f'[{i}]'
|
||||
if key not in dat:
|
||||
break
|
||||
list_dict[list_key].append(dat[key])
|
||||
del dat[key]
|
||||
i += 1
|
||||
return {**dat, **list_dict}
|
||||
Executable
+49
@@ -0,0 +1,49 @@
|
||||
#!/usr/bin/env python3
|
||||
import json
|
||||
import time
|
||||
import unittest
|
||||
import subprocess
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from system.hardware import TICI
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
|
||||
|
||||
class TestRawgpsd(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if not TICI:
|
||||
raise unittest.SkipTest
|
||||
|
||||
def tearDown(self):
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
def test_startup_time(self):
|
||||
for _ in range(5):
|
||||
sm = messaging.SubMaster(['qcomGnss'])
|
||||
managed_processes['rawgpsd'].start()
|
||||
|
||||
start_time = time.monotonic()
|
||||
for __ in range(10):
|
||||
sm.update(1 * 1000)
|
||||
if sm.updated['qcomGnss']:
|
||||
break
|
||||
assert sm.rcv_frame['qcomGnss'] > 0, "rawgpsd didn't start outputting messages in time"
|
||||
|
||||
et = time.monotonic() - start_time
|
||||
assert et < 5, f"rawgpsd took {et:.1f}s to start"
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
def test_turns_off_gnss(self):
|
||||
for s in (0.1, 0.5, 1, 5):
|
||||
managed_processes['rawgpsd'].start()
|
||||
time.sleep(s)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
|
||||
loc_status = json.loads(ls)
|
||||
assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Executable
+4
@@ -0,0 +1,4 @@
|
||||
#!/bin/sh
|
||||
cd "$(dirname "$0")"
|
||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
|
||||
exec ./_sensord
|
||||
@@ -0,0 +1,78 @@
|
||||
#include "bmx055_accel.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int BMX055_Accel::init() {
|
||||
int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
// bmx055 accel has a 1.3ms wakeup time from deep suspend mode
|
||||
util::sleep_for(10);
|
||||
|
||||
// High bandwidth
|
||||
// ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
|
||||
// if (ret < 0) {
|
||||
// goto fail;
|
||||
// }
|
||||
|
||||
// Low bandwidth
|
||||
ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int BMX055_Accel::shutdown() {
|
||||
// enter deep suspend mode (lowest power mode)
|
||||
int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not move BMX055 ACCEL in deep suspend mode!")
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[6];
|
||||
int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
|
||||
assert(len == 6);
|
||||
|
||||
// 12 bit = +-2g
|
||||
float scale = 9.81 * 2.0f / (1 << 11);
|
||||
float x = -read_12_bit(buffer[0], buffer[1]) * scale;
|
||||
float y = -read_12_bit(buffer[2], buffer[3]) * scale;
|
||||
float z = read_12_bit(buffer[4], buffer[5]) * scale;
|
||||
|
||||
auto event = msg.initEvent().initAccelerometer2();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_ACCELEROMETER);
|
||||
event.setType(SENSOR_TYPE_ACCELEROMETER);
|
||||
event.setTimestamp(start_time);
|
||||
|
||||
float xyz[] = {x, y, z};
|
||||
auto svec = event.initAcceleration();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define BMX055_ACCEL_I2C_ADDR 0x18
|
||||
|
||||
// Registers of the chip
|
||||
#define BMX055_ACCEL_I2C_REG_ID 0x00
|
||||
#define BMX055_ACCEL_I2C_REG_X_LSB 0x02
|
||||
#define BMX055_ACCEL_I2C_REG_TEMP 0x08
|
||||
#define BMX055_ACCEL_I2C_REG_BW 0x10
|
||||
#define BMX055_ACCEL_I2C_REG_PMU 0x11
|
||||
#define BMX055_ACCEL_I2C_REG_HBW 0x13
|
||||
#define BMX055_ACCEL_I2C_REG_FIFO 0x3F
|
||||
|
||||
// Constants
|
||||
#define BMX055_ACCEL_CHIP_ID 0xFA
|
||||
|
||||
#define BMX055_ACCEL_HBW_ENABLE 0b10000000
|
||||
#define BMX055_ACCEL_HBW_DISABLE 0b00000000
|
||||
#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000
|
||||
#define BMX055_ACCEL_NORMAL_MODE 0b00000000
|
||||
|
||||
#define BMX055_ACCEL_BW_7_81HZ 0b01000
|
||||
#define BMX055_ACCEL_BW_15_63HZ 0b01001
|
||||
#define BMX055_ACCEL_BW_31_25HZ 0b01010
|
||||
#define BMX055_ACCEL_BW_62_5HZ 0b01011
|
||||
#define BMX055_ACCEL_BW_125HZ 0b01100
|
||||
#define BMX055_ACCEL_BW_250HZ 0b01101
|
||||
#define BMX055_ACCEL_BW_500HZ 0b01110
|
||||
#define BMX055_ACCEL_BW_1000HZ 0b01111
|
||||
|
||||
class BMX055_Accel : public I2CSensor {
|
||||
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
|
||||
public:
|
||||
BMX055_Accel(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,88 @@
|
||||
#include "bmx055_gyro.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
||||
|
||||
|
||||
BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int BMX055_Gyro::init() {
|
||||
int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
// bmx055 gyro has a 30ms wakeup time from deep suspend mode
|
||||
util::sleep_for(50);
|
||||
|
||||
// High bandwidth
|
||||
// ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
|
||||
// if (ret < 0) {
|
||||
// goto fail;
|
||||
// }
|
||||
|
||||
// Low bandwidth
|
||||
ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// 116 Hz filter
|
||||
ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// +- 125 deg/s range
|
||||
ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int BMX055_Gyro::shutdown() {
|
||||
// enter deep suspend mode (lowest power mode)
|
||||
int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not move BMX055 GYRO in deep suspend mode!")
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[6];
|
||||
int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
|
||||
assert(len == 6);
|
||||
|
||||
// 16 bit = +- 125 deg/s
|
||||
float scale = 125.0f / (1 << 15);
|
||||
float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
|
||||
float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
|
||||
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
|
||||
|
||||
auto event = msg.initEvent().initGyroscope2();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
|
||||
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
|
||||
event.setTimestamp(start_time);
|
||||
|
||||
float xyz[] = {x, y, z};
|
||||
auto svec = event.initGyroUncalibrated();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define BMX055_GYRO_I2C_ADDR 0x68
|
||||
|
||||
// Registers of the chip
|
||||
#define BMX055_GYRO_I2C_REG_ID 0x00
|
||||
#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
|
||||
#define BMX055_GYRO_I2C_REG_RANGE 0x0F
|
||||
#define BMX055_GYRO_I2C_REG_BW 0x10
|
||||
#define BMX055_GYRO_I2C_REG_LPM1 0x11
|
||||
#define BMX055_GYRO_I2C_REG_HBW 0x13
|
||||
#define BMX055_GYRO_I2C_REG_FIFO 0x3F
|
||||
|
||||
// Constants
|
||||
#define BMX055_GYRO_CHIP_ID 0x0F
|
||||
|
||||
#define BMX055_GYRO_HBW_ENABLE 0b10000000
|
||||
#define BMX055_GYRO_HBW_DISABLE 0b00000000
|
||||
#define BMX055_GYRO_DEEP_SUSPEND 0b00100000
|
||||
#define BMX055_GYRO_NORMAL_MODE 0b00000000
|
||||
|
||||
#define BMX055_GYRO_RANGE_2000 0b000
|
||||
#define BMX055_GYRO_RANGE_1000 0b001
|
||||
#define BMX055_GYRO_RANGE_500 0b010
|
||||
#define BMX055_GYRO_RANGE_250 0b011
|
||||
#define BMX055_GYRO_RANGE_125 0b100
|
||||
|
||||
#define BMX055_GYRO_BW_116HZ 0b0010
|
||||
|
||||
|
||||
class BMX055_Gyro : public I2CSensor {
|
||||
uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;}
|
||||
public:
|
||||
BMX055_Gyro(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,256 @@
|
||||
#include "bmx055_magn.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
|
||||
static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
|
||||
uint16_t process_comp_x0 = data_rhall;
|
||||
int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
|
||||
uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
|
||||
int16_t retval = ((int16_t)process_comp_x2);
|
||||
int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
|
||||
int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
|
||||
int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
|
||||
int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
|
||||
int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
|
||||
int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
|
||||
int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
|
||||
int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
|
||||
retval = ((int16_t)(process_comp_x10 / 8192));
|
||||
retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
|
||||
|
||||
return retval;
|
||||
}
|
||||
|
||||
static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
|
||||
uint16_t process_comp_y0 = trim_data.dig_xyz1;
|
||||
int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
|
||||
uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
|
||||
int16_t retval = ((int16_t)process_comp_y2);
|
||||
int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
|
||||
int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
|
||||
int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
|
||||
int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
|
||||
int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
|
||||
int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
|
||||
int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
|
||||
retval = (int16_t)(process_comp_y9 / 8192);
|
||||
retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
|
||||
|
||||
return retval;
|
||||
}
|
||||
|
||||
static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
|
||||
int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
|
||||
int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
|
||||
int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
|
||||
int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
|
||||
int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
|
||||
int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
|
||||
|
||||
/* saturate result to +/- 2 micro-tesla */
|
||||
retval = std::clamp(retval, -32767, 32767);
|
||||
|
||||
/* Conversion of LSB to micro-tesla*/
|
||||
retval = retval / 16;
|
||||
|
||||
return (int16_t)retval;
|
||||
}
|
||||
|
||||
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int BMX055_Magn::init() {
|
||||
uint8_t trim_x1y1[2] = {0};
|
||||
uint8_t trim_x2y2[2] = {0};
|
||||
uint8_t trim_xy1xy2[2] = {0};
|
||||
uint8_t trim_z1[2] = {0};
|
||||
uint8_t trim_z2[2] = {0};
|
||||
uint8_t trim_z3[2] = {0};
|
||||
uint8_t trim_z4[2] = {0};
|
||||
uint8_t trim_xyz1[2] = {0};
|
||||
|
||||
// suspend -> sleep
|
||||
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
|
||||
if(ret < 0) {
|
||||
LOGE("Enabling power failed: %d", ret);
|
||||
goto fail;
|
||||
}
|
||||
util::sleep_for(5); // wait until the chip is powered on
|
||||
|
||||
ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
|
||||
if (ret == -1) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Load magnetometer trim
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
|
||||
if(ret < 0) goto fail;
|
||||
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
|
||||
if(ret < 0) goto fail;
|
||||
|
||||
// Read trim data
|
||||
trim_data.dig_x1 = trim_x1y1[0];
|
||||
trim_data.dig_y1 = trim_x1y1[1];
|
||||
|
||||
trim_data.dig_x2 = trim_x2y2[0];
|
||||
trim_data.dig_y2 = trim_x2y2[1];
|
||||
|
||||
trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
|
||||
trim_data.dig_xy2 = trim_xy1xy2[0];
|
||||
|
||||
trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
|
||||
trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
|
||||
trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
|
||||
trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
|
||||
|
||||
trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
|
||||
assert(trim_data.dig_xyz1 != 0);
|
||||
|
||||
perform_self_test();
|
||||
|
||||
// f_max = 1 / (145us * nXY + 500us * NZ + 980us)
|
||||
// Chose NXY = 7, NZ = 12, which gives 125 Hz,
|
||||
// and has the same ratio as the high accuracy preset
|
||||
ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int BMX055_Magn::shutdown() {
|
||||
// move to suspend mode
|
||||
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not move BMX055 MAGN in suspend mode!")
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool BMX055_Magn::perform_self_test() {
|
||||
uint8_t buffer[8];
|
||||
int16_t x, y;
|
||||
int16_t neg_z, pos_z;
|
||||
|
||||
// Increase z reps for less false positives (~30 Hz ODR)
|
||||
set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
|
||||
set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
|
||||
|
||||
// Clean existing measurement
|
||||
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
|
||||
|
||||
uint8_t forced = BMX055_MAGN_FORCED;
|
||||
|
||||
// Negative current
|
||||
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
|
||||
util::sleep_for(100);
|
||||
|
||||
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
|
||||
parse_xyz(buffer, &x, &y, &neg_z);
|
||||
|
||||
// Positive current
|
||||
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
|
||||
util::sleep_for(100);
|
||||
|
||||
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
|
||||
parse_xyz(buffer, &x, &y, &pos_z);
|
||||
|
||||
// Put back in normal mode
|
||||
set_register(BMX055_MAGN_I2C_REG_MAG, 0);
|
||||
|
||||
int16_t diff = pos_z - neg_z;
|
||||
bool passed = (diff > 180) && (diff < 240);
|
||||
|
||||
if (!passed) {
|
||||
LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
|
||||
}
|
||||
|
||||
return passed;
|
||||
}
|
||||
|
||||
bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
|
||||
bool ready = buffer[6] & 0x1;
|
||||
if (ready) {
|
||||
int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
|
||||
int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
|
||||
int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
|
||||
uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
|
||||
assert(data_r != 0);
|
||||
|
||||
*x = compensate_x(trim_data, mdata_x, data_r);
|
||||
*y = compensate_y(trim_data, mdata_y, data_r);
|
||||
*z = compensate_z(trim_data, mdata_z, data_r);
|
||||
}
|
||||
return ready;
|
||||
}
|
||||
|
||||
|
||||
bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[8];
|
||||
int16_t _x, _y, x, y, z;
|
||||
|
||||
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
bool parsed = parse_xyz(buffer, &_x, &_y, &z);
|
||||
if (parsed) {
|
||||
|
||||
auto event = msg.initEvent().initMagnetometer();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
|
||||
event.setVersion(2);
|
||||
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
|
||||
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
|
||||
event.setTimestamp(start_time);
|
||||
|
||||
// Move magnetometer into same reference frame as accel/gryo
|
||||
x = -_y;
|
||||
y = _x;
|
||||
|
||||
// Axis convention
|
||||
x = -x;
|
||||
y = -y;
|
||||
|
||||
float xyz[] = {(float)x, (float)y, (float)z};
|
||||
auto svec = event.initMagneticUncalibrated();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
}
|
||||
|
||||
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
|
||||
// up to 30 Hz. Therefore we put in forced mode, and request measurements
|
||||
// at a 100 Hz. When reading the registers we have to check the ready bit
|
||||
// To verify the measurement was completed this cycle.
|
||||
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
|
||||
|
||||
return parsed;
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
#pragma once
|
||||
#include <tuple>
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define BMX055_MAGN_I2C_ADDR 0x10
|
||||
|
||||
// Registers of the chip
|
||||
#define BMX055_MAGN_I2C_REG_ID 0x40
|
||||
#define BMX055_MAGN_I2C_REG_PWR_0 0x4B
|
||||
#define BMX055_MAGN_I2C_REG_MAG 0x4C
|
||||
#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
|
||||
#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
|
||||
#define BMX055_MAGN_I2C_REG_REPXY 0x51
|
||||
#define BMX055_MAGN_I2C_REG_REPZ 0x52
|
||||
|
||||
#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63
|
||||
#define BMX055_MAGN_I2C_REG_DIG_X2 0x64
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B
|
||||
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
|
||||
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E
|
||||
#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F
|
||||
#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70
|
||||
#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71
|
||||
|
||||
// Constants
|
||||
#define BMX055_MAGN_CHIP_ID 0x32
|
||||
#define BMX055_MAGN_FORCED (0b01 << 1)
|
||||
|
||||
struct trim_data_t {
|
||||
int8_t dig_x1;
|
||||
int8_t dig_y1;
|
||||
int8_t dig_x2;
|
||||
int8_t dig_y2;
|
||||
uint16_t dig_z1;
|
||||
int16_t dig_z2;
|
||||
int16_t dig_z3;
|
||||
int16_t dig_z4;
|
||||
uint8_t dig_xy1;
|
||||
int8_t dig_xy2;
|
||||
uint16_t dig_xyz1;
|
||||
};
|
||||
|
||||
|
||||
class BMX055_Magn : public I2CSensor{
|
||||
uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
|
||||
trim_data_t trim_data = {0};
|
||||
bool perform_self_test();
|
||||
bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
|
||||
public:
|
||||
BMX055_Magn(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,31 @@
|
||||
#include "bmx055_temp.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "system/sensord/sensors/bmx055_accel.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int BMX055_Temp::init() {
|
||||
return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
|
||||
}
|
||||
|
||||
bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[1];
|
||||
int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
|
||||
|
||||
auto event = msg.initEvent().initTemperatureSensor();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
|
||||
event.setVersion(1);
|
||||
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
|
||||
event.setTimestamp(start_time);
|
||||
event.setTemperature(temp);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/bmx055_accel.h"
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
class BMX055_Temp : public I2CSensor {
|
||||
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
|
||||
public:
|
||||
BMX055_Temp(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown() { return 0; }
|
||||
};
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#define SENSOR_ACCELEROMETER 1
|
||||
#define SENSOR_MAGNETOMETER 2
|
||||
#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
|
||||
#define SENSOR_GYRO 4
|
||||
#define SENSOR_GYRO_UNCALIBRATED 5
|
||||
#define SENSOR_LIGHT 7
|
||||
|
||||
#define SENSOR_TYPE_ACCELEROMETER 1
|
||||
#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
|
||||
#define SENSOR_TYPE_GYROSCOPE 4
|
||||
#define SENSOR_TYPE_LIGHT 5
|
||||
#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
|
||||
#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
|
||||
#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
|
||||
#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "file_sensor.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
FileSensor::FileSensor(std::string filename) : file(filename) {}
|
||||
|
||||
int FileSensor::init() {
|
||||
return file.is_open() ? 0 : 1;
|
||||
}
|
||||
|
||||
FileSensor::~FileSensor() {
|
||||
file.close();
|
||||
}
|
||||
|
||||
bool FileSensor::has_interrupt_enabled() {
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "system/sensord/sensors/sensor.h"
|
||||
|
||||
class FileSensor : public Sensor {
|
||||
protected:
|
||||
std::ifstream file;
|
||||
|
||||
public:
|
||||
FileSensor(std::string filename);
|
||||
~FileSensor();
|
||||
int init();
|
||||
bool has_interrupt_enabled();
|
||||
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
|
||||
};
|
||||
@@ -0,0 +1,50 @@
|
||||
#include "i2c_sensor.h"
|
||||
|
||||
int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
|
||||
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
|
||||
return int16_t(combined) / (1 << 4);
|
||||
}
|
||||
|
||||
int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
|
||||
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb);
|
||||
return int16_t(combined);
|
||||
}
|
||||
|
||||
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
|
||||
uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
|
||||
return int32_t(combined) / (1 << 4);
|
||||
}
|
||||
|
||||
I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
|
||||
bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
|
||||
|
||||
I2CSensor::~I2CSensor() {
|
||||
if (gpio_fd != -1) {
|
||||
close(gpio_fd);
|
||||
}
|
||||
}
|
||||
|
||||
int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) {
|
||||
return bus->read_register(get_device_address(), register_address, buffer, len);
|
||||
}
|
||||
|
||||
int I2CSensor::set_register(uint register_address, uint8_t data) {
|
||||
return bus->set_register(get_device_address(), register_address, data);
|
||||
}
|
||||
|
||||
int I2CSensor::init_gpio() {
|
||||
if (shared_gpio || gpio_nr == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
|
||||
if (gpio_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool I2CSensor::has_interrupt_enabled() {
|
||||
return gpio_nr != 0;
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <unistd.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/i2c.h"
|
||||
#include "common/gpio.h"
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "system/sensord/sensors/constants.h"
|
||||
#include "system/sensord/sensors/sensor.h"
|
||||
|
||||
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
|
||||
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
|
||||
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
|
||||
|
||||
|
||||
class I2CSensor : public Sensor {
|
||||
private:
|
||||
I2CBus *bus;
|
||||
int gpio_nr;
|
||||
bool shared_gpio;
|
||||
virtual uint8_t get_device_address() = 0;
|
||||
|
||||
public:
|
||||
I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
|
||||
~I2CSensor();
|
||||
int read_register(uint register_address, uint8_t *buffer, uint8_t len);
|
||||
int set_register(uint register_address, uint8_t data);
|
||||
int init_gpio();
|
||||
bool has_interrupt_enabled();
|
||||
virtual int init() = 0;
|
||||
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
|
||||
virtual int shutdown() = 0;
|
||||
|
||||
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
|
||||
uint8_t chip_id = 0;
|
||||
int ret = read_register(address, &chip_id, 1);
|
||||
if (ret < 0) {
|
||||
LOGE("Reading chip ID failed: %d", ret);
|
||||
return -1;
|
||||
}
|
||||
for (int i = 0; i < expected_ids.size(); ++i) {
|
||||
if (chip_id == expected_ids[i]) return chip_id;
|
||||
}
|
||||
LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
|
||||
return -1;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,27 @@
|
||||
#include "light_sensor.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "common/timing.h"
|
||||
#include "system/sensord/sensors/constants.h"
|
||||
|
||||
LightSensor::LightSensor(std::string filename) : FileSensor(filename) {}
|
||||
|
||||
bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
file.clear();
|
||||
file.seekg(0);
|
||||
|
||||
int value;
|
||||
file >> value;
|
||||
|
||||
auto event = msg.initEvent().initLightSensor();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::RPR0521);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_LIGHT);
|
||||
event.setType(SENSOR_TYPE_LIGHT);
|
||||
event.setTimestamp(start_time);
|
||||
event.setLight(value);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
#pragma once
|
||||
#include "file_sensor.h"
|
||||
|
||||
class LightSensor : public FileSensor {
|
||||
public:
|
||||
LightSensor(std::string filename);
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown() { return 0; }
|
||||
};
|
||||
@@ -0,0 +1,250 @@
|
||||
#include "lsm6ds3_accel.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
|
||||
I2CSensor(bus, gpio_nr, shared_gpio) {}
|
||||
|
||||
void LSM6DS3_Accel::wait_for_data_ready() {
|
||||
uint8_t drdy = 0;
|
||||
uint8_t buffer[6];
|
||||
|
||||
do {
|
||||
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
|
||||
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
|
||||
} while (drdy == 0);
|
||||
|
||||
read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
|
||||
}
|
||||
|
||||
void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
|
||||
uint8_t drdy = 0;
|
||||
uint8_t buffer[6];
|
||||
|
||||
float scaling = 0.061f;
|
||||
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
|
||||
scaling = 0.122f;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
do {
|
||||
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
|
||||
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
|
||||
} while (drdy == 0);
|
||||
|
||||
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
for (int j = 0; j < 3; j++) {
|
||||
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
out_buf[i] /= 5.0f;
|
||||
}
|
||||
}
|
||||
|
||||
int LSM6DS3_Accel::self_test(int test_type) {
|
||||
float val_st_off[3] = {0};
|
||||
float val_st_on[3] = {0};
|
||||
float test_val[3] = {0};
|
||||
uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
|
||||
|
||||
// prepare sensor for self-test
|
||||
|
||||
// enable block data update and automatic increment
|
||||
int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
|
||||
ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
|
||||
}
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// wait for stable output, and discard first values
|
||||
util::sleep_for(100);
|
||||
wait_for_data_ready();
|
||||
read_and_avg_data(val_st_off);
|
||||
|
||||
// enable Self Test positive (or negative)
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// wait for stable output, and discard first values
|
||||
util::sleep_for(100);
|
||||
wait_for_data_ready();
|
||||
read_and_avg_data(val_st_on);
|
||||
|
||||
// disable sensor
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// disable self test
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// calculate the mg values for self test
|
||||
for (int i = 0; i < 3; i++) {
|
||||
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
|
||||
}
|
||||
|
||||
// verify test result
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
|
||||
(test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int LSM6DS3_Accel::init() {
|
||||
uint8_t value = 0;
|
||||
bool do_self_test = false;
|
||||
|
||||
const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
|
||||
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
|
||||
do_self_test = true;
|
||||
}
|
||||
|
||||
int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
|
||||
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
|
||||
}
|
||||
|
||||
ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
|
||||
if (ret < 0) {
|
||||
LOGE("LSM6DS3 accel positive self-test failed!");
|
||||
if (do_self_test) goto fail;
|
||||
}
|
||||
|
||||
ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
|
||||
if (ret < 0) {
|
||||
LOGE("LSM6DS3 accel negative self-test failed!");
|
||||
if (do_self_test) goto fail;
|
||||
}
|
||||
|
||||
ret = init_gpio();
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// enable continuous update, and automatic increase
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// enable data ready interrupt for accel on INT1
|
||||
// (without resetting existing interrupts)
|
||||
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int LSM6DS3_Accel::shutdown() {
|
||||
int ret = 0;
|
||||
|
||||
// disable data ready interrupt for accel on INT1
|
||||
uint8_t value = 0;
|
||||
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not disable lsm6ds3 acceleration interrupt!")
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// enable power-down mode
|
||||
value = 0;
|
||||
ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value &= 0x0F;
|
||||
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not power-down lsm6ds3 accelerometer!")
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
|
||||
// INT1 shared with gyro, check STATUS_REG who triggered
|
||||
uint8_t status_reg = 0;
|
||||
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
|
||||
if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t buffer[6];
|
||||
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float scale = 9.81 * 2.0f / (1 << 15);
|
||||
float x = read_16_bit(buffer[0], buffer[1]) * scale;
|
||||
float y = read_16_bit(buffer[2], buffer[3]) * scale;
|
||||
float z = read_16_bit(buffer[4], buffer[5]) * scale;
|
||||
|
||||
auto event = msg.initEvent().initAccelerometer();
|
||||
event.setSource(source);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_ACCELEROMETER);
|
||||
event.setType(SENSOR_TYPE_ACCELEROMETER);
|
||||
event.setTimestamp(ts);
|
||||
|
||||
float xyz[] = {y, -x, z};
|
||||
auto svec = event.initAcceleration();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define LSM6DS3_ACCEL_I2C_ADDR 0x6A
|
||||
|
||||
// Registers of the chip
|
||||
#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B
|
||||
#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F
|
||||
#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
|
||||
#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10
|
||||
#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12
|
||||
#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14
|
||||
#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18
|
||||
#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E
|
||||
#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
|
||||
|
||||
// Constants
|
||||
#define LSM6DS3_ACCEL_CHIP_ID 0x69
|
||||
#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A
|
||||
#define LSM6DS3_ACCEL_FS_4G (0b10 << 2)
|
||||
#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4)
|
||||
#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4)
|
||||
#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1
|
||||
#define LSM6DS3_ACCEL_DRDY_XLDA 0b1
|
||||
#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
|
||||
#define LSM6DS3_ACCEL_IF_INC 0b00000100
|
||||
#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100
|
||||
#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000
|
||||
#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01
|
||||
#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10
|
||||
#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
|
||||
#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
|
||||
|
||||
class LSM6DS3_Accel : public I2CSensor {
|
||||
uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
|
||||
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
|
||||
|
||||
// self test functions
|
||||
int self_test(int test_type);
|
||||
void wait_for_data_ready();
|
||||
void read_and_avg_data(float* val_st_off);
|
||||
public:
|
||||
LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,233 @@
|
||||
#include "lsm6ds3_gyro.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
||||
|
||||
LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
|
||||
I2CSensor(bus, gpio_nr, shared_gpio) {}
|
||||
|
||||
void LSM6DS3_Gyro::wait_for_data_ready() {
|
||||
uint8_t drdy = 0;
|
||||
uint8_t buffer[6];
|
||||
|
||||
do {
|
||||
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
|
||||
drdy &= LSM6DS3_GYRO_DRDY_GDA;
|
||||
} while (drdy == 0);
|
||||
|
||||
read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
|
||||
}
|
||||
|
||||
void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
|
||||
uint8_t drdy = 0;
|
||||
uint8_t buffer[6];
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
do {
|
||||
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
|
||||
drdy &= LSM6DS3_GYRO_DRDY_GDA;
|
||||
} while (drdy == 0);
|
||||
|
||||
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
for (int j = 0; j < 3; j++) {
|
||||
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the mg average values
|
||||
for (int i = 0; i < 3; i++) {
|
||||
out_buf[i] /= 5.0f;
|
||||
}
|
||||
}
|
||||
|
||||
int LSM6DS3_Gyro::self_test(int test_type) {
|
||||
float val_st_off[3] = {0};
|
||||
float val_st_on[3] = {0};
|
||||
float test_val[3] = {0};
|
||||
|
||||
// prepare sensor for self-test
|
||||
|
||||
// full scale: 2000dps, ODR: 208Hz
|
||||
int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// wait for stable output, and discard first values
|
||||
util::sleep_for(150);
|
||||
wait_for_data_ready();
|
||||
read_and_avg_data(val_st_off);
|
||||
|
||||
// enable Self Test positive (or negative)
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// wait for stable output, and discard first values
|
||||
util::sleep_for(50);
|
||||
wait_for_data_ready();
|
||||
read_and_avg_data(val_st_on);
|
||||
|
||||
// disable sensor
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// disable self test
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// calculate the mg values for self test
|
||||
for (int i = 0; i < 3; i++) {
|
||||
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
|
||||
}
|
||||
|
||||
// verify test result
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
|
||||
(test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int LSM6DS3_Gyro::init() {
|
||||
uint8_t value = 0;
|
||||
bool do_self_test = false;
|
||||
|
||||
const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
|
||||
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
|
||||
do_self_test = true;
|
||||
}
|
||||
|
||||
int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
|
||||
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
|
||||
}
|
||||
|
||||
ret = init_gpio();
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
|
||||
if (ret < 0 ) {
|
||||
LOGE("LSM6DS3 gyro positive self-test failed!");
|
||||
if (do_self_test) goto fail;
|
||||
}
|
||||
|
||||
ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
|
||||
if (ret < 0) {
|
||||
LOGE("LSM6DS3 gyro negative self-test failed!");
|
||||
if (do_self_test) goto fail;
|
||||
}
|
||||
|
||||
// TODO: set scale. Default is +- 250 deg/s
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// enable data ready interrupt for gyro on INT1
|
||||
// (without resetting existing interrupts)
|
||||
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value |= LSM6DS3_GYRO_INT1_DRDY_G;
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int LSM6DS3_Gyro::shutdown() {
|
||||
int ret = 0;
|
||||
|
||||
// disable data ready interrupt for gyro on INT1
|
||||
uint8_t value = 0;
|
||||
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not disable lsm6ds3 gyroscope interrupt!")
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// enable power-down mode
|
||||
value = 0;
|
||||
ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value &= 0x0F;
|
||||
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
|
||||
if (ret < 0) {
|
||||
LOGE("Could not power-down lsm6ds3 gyroscope!")
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
|
||||
// INT1 shared with accel, check STATUS_REG who triggered
|
||||
uint8_t status_reg = 0;
|
||||
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
|
||||
if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t buffer[6];
|
||||
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float scale = 8.75 / 1000.0;
|
||||
float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
|
||||
float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
|
||||
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
|
||||
|
||||
auto event = msg.initEvent().initGyroscope();
|
||||
event.setSource(source);
|
||||
event.setVersion(2);
|
||||
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
|
||||
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
|
||||
event.setTimestamp(ts);
|
||||
|
||||
float xyz[] = {y, -x, z};
|
||||
auto svec = event.initGyroUncalibrated();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define LSM6DS3_GYRO_I2C_ADDR 0x6A
|
||||
|
||||
// Registers of the chip
|
||||
#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B
|
||||
#define LSM6DS3_GYRO_I2C_REG_ID 0x0F
|
||||
#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
|
||||
#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11
|
||||
#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14
|
||||
#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E
|
||||
#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22
|
||||
#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2)
|
||||
#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2)
|
||||
|
||||
// Constants
|
||||
#define LSM6DS3_GYRO_CHIP_ID 0x69
|
||||
#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A
|
||||
#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2)
|
||||
#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4)
|
||||
#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4)
|
||||
#define LSM6DS3_GYRO_INT1_DRDY_G 0b10
|
||||
#define LSM6DS3_GYRO_DRDY_GDA 0b10
|
||||
#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
|
||||
#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
|
||||
#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
|
||||
|
||||
|
||||
class LSM6DS3_Gyro : public I2CSensor {
|
||||
uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
|
||||
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
|
||||
|
||||
// self test functions
|
||||
int self_test(int test_type);
|
||||
void wait_for_data_ready();
|
||||
void read_and_avg_data(float* val_st_off);
|
||||
public:
|
||||
LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,37 @@
|
||||
#include "lsm6ds3_temp.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int LSM6DS3_Temp::init() {
|
||||
int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
|
||||
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[2];
|
||||
int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
|
||||
float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
|
||||
|
||||
auto event = msg.initEvent().initTemperatureSensor();
|
||||
event.setSource(source);
|
||||
event.setVersion(1);
|
||||
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
|
||||
event.setTimestamp(start_time);
|
||||
event.setTemperature(temp);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define LSM6DS3_TEMP_I2C_ADDR 0x6A
|
||||
|
||||
// Registers of the chip
|
||||
#define LSM6DS3_TEMP_I2C_REG_ID 0x0F
|
||||
#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20
|
||||
|
||||
// Constants
|
||||
#define LSM6DS3_TEMP_CHIP_ID 0x69
|
||||
#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A
|
||||
|
||||
|
||||
class LSM6DS3_Temp : public I2CSensor {
|
||||
uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
|
||||
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
|
||||
|
||||
public:
|
||||
LSM6DS3_Temp(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown() { return 0; }
|
||||
};
|
||||
@@ -0,0 +1,94 @@
|
||||
#include "mmc5603nj_magn.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int MMC5603NJ_Magn::init() {
|
||||
int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
|
||||
if (ret == -1) return -1;
|
||||
|
||||
// Set 100 Hz
|
||||
ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Set BW to 0b01 for 1-150 Hz operation
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Set compute measurement rate
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Enable continuous mode, set every 100 measurements
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int MMC5603NJ_Magn::shutdown() {
|
||||
int ret = 0;
|
||||
|
||||
// disable auto reset of measurements
|
||||
uint8_t value = 0;
|
||||
ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// set ODR to 0 to leave continuous mode
|
||||
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
return ret;
|
||||
|
||||
fail:
|
||||
LOGE("Could not disable mmc5603nj auto set reset")
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[9];
|
||||
int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float scale = 1.0 / 16384.0;
|
||||
float x = read_20_bit(buffer[6], buffer[1], buffer[0]) * scale;
|
||||
float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale;
|
||||
float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale;
|
||||
|
||||
auto event = msg.initEvent().initMagnetometer();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
|
||||
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
|
||||
event.setTimestamp(start_time);
|
||||
|
||||
float xyz[] = {x, y, z};
|
||||
auto svec = event.initMagneticUncalibrated();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
#include "system/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define MMC5603NJ_I2C_ADDR 0x30
|
||||
|
||||
// Registers of the chip
|
||||
#define MMC5603NJ_I2C_REG_XOUT0 0x00
|
||||
#define MMC5603NJ_I2C_REG_ODR 0x1A
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D
|
||||
#define MMC5603NJ_I2C_REG_ID 0x39
|
||||
|
||||
// Constants
|
||||
#define MMC5603NJ_CHIP_ID 0x10
|
||||
#define MMC5603NJ_CMM_FREQ_EN (1 << 7)
|
||||
#define MMC5603NJ_AUTO_SR_EN (1 << 5)
|
||||
#define MMC5603NJ_CMM_EN (1 << 4)
|
||||
#define MMC5603NJ_EN_PRD_SET (1 << 3)
|
||||
|
||||
class MMC5603NJ_Magn : public I2CSensor {
|
||||
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
|
||||
public:
|
||||
MMC5603NJ_Magn(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
class Sensor {
|
||||
public:
|
||||
int gpio_fd = -1;
|
||||
uint64_t init_delay = 500e6; // default dealy 500ms
|
||||
virtual ~Sensor() {};
|
||||
virtual int init() = 0;
|
||||
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
|
||||
virtual bool has_interrupt_enabled() = 0;
|
||||
virtual int shutdown() = 0;
|
||||
|
||||
virtual bool is_data_valid(uint64_t st, uint64_t ct) {
|
||||
return (ct - st) > init_delay;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,213 @@
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <poll.h>
|
||||
#include <linux/gpio.h>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/i2c.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
#include "system/sensord/sensors/bmx055_accel.h"
|
||||
#include "system/sensord/sensors/bmx055_gyro.h"
|
||||
#include "system/sensord/sensors/bmx055_magn.h"
|
||||
#include "system/sensord/sensors/bmx055_temp.h"
|
||||
#include "system/sensord/sensors/constants.h"
|
||||
#include "system/sensord/sensors/light_sensor.h"
|
||||
#include "system/sensord/sensors/lsm6ds3_accel.h"
|
||||
#include "system/sensord/sensors/lsm6ds3_gyro.h"
|
||||
#include "system/sensord/sensors/lsm6ds3_temp.h"
|
||||
#include "system/sensord/sensors/mmc5603nj_magn.h"
|
||||
#include "system/sensord/sensors/sensor.h"
|
||||
|
||||
#define I2C_BUS_IMU 1
|
||||
|
||||
ExitHandler do_exit;
|
||||
uint64_t init_ts = 0;
|
||||
|
||||
void interrupt_loop(std::vector<Sensor *>& sensors,
|
||||
std::map<Sensor*, std::string>& sensor_service)
|
||||
{
|
||||
PubMaster pm_int({"gyroscope", "accelerometer"});
|
||||
|
||||
int fd = sensors[0]->gpio_fd;
|
||||
struct pollfd fd_list[1] = {0};
|
||||
fd_list[0].fd = fd;
|
||||
fd_list[0].events = POLLIN | POLLPRI;
|
||||
|
||||
while (!do_exit) {
|
||||
int err = poll(fd_list, 1, 100);
|
||||
if (err == -1) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
return;
|
||||
} else if (err == 0) {
|
||||
LOGE("poll timed out");
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) {
|
||||
LOGE("no poll events set");
|
||||
continue;
|
||||
}
|
||||
|
||||
// Read all events
|
||||
struct gpioevent_data evdata[16];
|
||||
err = read(fd, evdata, sizeof(evdata));
|
||||
if (err < 0 || err % sizeof(*evdata) != 0) {
|
||||
LOGE("error reading event data %d", err);
|
||||
continue;
|
||||
}
|
||||
|
||||
int num_events = err / sizeof(*evdata);
|
||||
uint64_t offset = nanos_since_epoch() - nanos_since_boot();
|
||||
uint64_t ts = evdata[num_events - 1].timestamp - offset;
|
||||
|
||||
for (Sensor *sensor : sensors) {
|
||||
MessageBuilder msg;
|
||||
if (!sensor->get_event(msg, ts)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!sensor->is_data_valid(init_ts, ts)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
pm_int.send(sensor_service[sensor].c_str(), msg);
|
||||
}
|
||||
}
|
||||
|
||||
// poweroff sensors, disable interrupts
|
||||
for (Sensor *sensor : sensors) {
|
||||
sensor->shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
int sensor_loop(I2CBus *i2c_bus_imu) {
|
||||
BMX055_Accel bmx055_accel(i2c_bus_imu);
|
||||
BMX055_Gyro bmx055_gyro(i2c_bus_imu);
|
||||
BMX055_Magn bmx055_magn(i2c_bus_imu);
|
||||
BMX055_Temp bmx055_temp(i2c_bus_imu);
|
||||
|
||||
LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu, GPIO_LSM_INT);
|
||||
LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu, GPIO_LSM_INT, true); // GPIO shared with accel
|
||||
LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu);
|
||||
|
||||
MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu);
|
||||
|
||||
LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
|
||||
|
||||
std::map<Sensor*, std::string> sensor_service = {
|
||||
{&bmx055_accel, "accelerometer2"},
|
||||
{&bmx055_gyro, "gyroscope2"},
|
||||
{&bmx055_magn, "magnetometer"},
|
||||
{&bmx055_temp, "temperatureSensor"},
|
||||
|
||||
{&lsm6ds3_accel, "accelerometer"},
|
||||
{&lsm6ds3_gyro, "gyroscope"},
|
||||
{&lsm6ds3_temp, "temperatureSensor"},
|
||||
|
||||
{&mmc5603nj_magn, "magnetometer"},
|
||||
{&light, "lightSensor"}
|
||||
};
|
||||
|
||||
// Sensor init
|
||||
std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
|
||||
sensors_init.push_back({&bmx055_accel, false});
|
||||
sensors_init.push_back({&bmx055_gyro, false});
|
||||
sensors_init.push_back({&bmx055_magn, false});
|
||||
sensors_init.push_back({&bmx055_temp, false});
|
||||
|
||||
sensors_init.push_back({&lsm6ds3_accel, true});
|
||||
sensors_init.push_back({&lsm6ds3_gyro, true});
|
||||
sensors_init.push_back({&lsm6ds3_temp, true});
|
||||
|
||||
sensors_init.push_back({&mmc5603nj_magn, false});
|
||||
|
||||
sensors_init.push_back({&light, true});
|
||||
|
||||
bool has_magnetometer = false;
|
||||
|
||||
// Initialize sensors
|
||||
std::vector<Sensor *> sensors;
|
||||
for (auto &[sensor, required] : sensors_init) {
|
||||
int err = sensor->init();
|
||||
if (err < 0) {
|
||||
if (required) {
|
||||
LOGE("Error initializing sensors");
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
|
||||
if (sensor == &bmx055_magn || sensor == &mmc5603nj_magn) {
|
||||
has_magnetometer = true;
|
||||
}
|
||||
|
||||
if (!sensor->has_interrupt_enabled()) {
|
||||
sensors.push_back(sensor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!has_magnetometer) {
|
||||
LOGE("No magnetometer present");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// increase interrupt quality by pinning interrupt and process to core 1
|
||||
setpriority(PRIO_PROCESS, 0, -18);
|
||||
util::set_core_affinity({1});
|
||||
std::system("sudo su -c 'echo 1 > /proc/irq/336/smp_affinity_list'");
|
||||
|
||||
PubMaster pm_non_int({"gyroscope2", "accelerometer2", "temperatureSensor",
|
||||
"lightSensor", "magnetometer"});
|
||||
init_ts = nanos_since_boot();
|
||||
|
||||
// thread for reading events via interrupts
|
||||
std::vector<Sensor *> lsm_interrupt_sensors = {&lsm6ds3_accel, &lsm6ds3_gyro};
|
||||
std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors),
|
||||
std::ref(sensor_service));
|
||||
|
||||
// polling loop for non interrupt handled sensors
|
||||
while (!do_exit) {
|
||||
std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
|
||||
|
||||
for (Sensor *sensor : sensors) {
|
||||
MessageBuilder msg;
|
||||
if (!sensor->get_event(msg)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!sensor->is_data_valid(init_ts, nanos_since_boot())) {
|
||||
continue;
|
||||
}
|
||||
|
||||
pm_non_int.send(sensor_service[sensor].c_str(), msg);
|
||||
}
|
||||
|
||||
std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin));
|
||||
}
|
||||
|
||||
for (Sensor *sensor : sensors) {
|
||||
sensor->shutdown();
|
||||
}
|
||||
|
||||
lsm_interrupt_thread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
try {
|
||||
auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
|
||||
return sensor_loop(i2c_bus_imu.get());
|
||||
} catch (std::exception &e) {
|
||||
LOGE("I2CBus init failed");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
Executable
+63
@@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
import unittest
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal.services import service_list
|
||||
from common.gpio import gpio_read
|
||||
from selfdrive.test.helpers import with_processes
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
from system.hardware import TICI
|
||||
from system.hardware.tici.pins import GPIO
|
||||
|
||||
|
||||
# TODO: test TTFF when we have good A-GNSS
|
||||
class TestPigeond(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if not TICI:
|
||||
raise unittest.SkipTest
|
||||
|
||||
def tearDown(self):
|
||||
managed_processes['pigeond'].stop()
|
||||
|
||||
@with_processes(['pigeond'])
|
||||
def test_frequency(self):
|
||||
sm = messaging.SubMaster(['ubloxRaw'])
|
||||
|
||||
# setup time
|
||||
time.sleep(2)
|
||||
sm.update()
|
||||
|
||||
for _ in range(int(10 * service_list['ubloxRaw'].frequency)):
|
||||
sm.update()
|
||||
assert sm.all_checks()
|
||||
|
||||
def test_startup_time(self):
|
||||
for _ in range(5):
|
||||
sm = messaging.SubMaster(['ubloxRaw'])
|
||||
managed_processes['pigeond'].start()
|
||||
|
||||
start_time = time.monotonic()
|
||||
for __ in range(10):
|
||||
sm.update(1 * 1000)
|
||||
if sm.updated['ubloxRaw']:
|
||||
break
|
||||
assert sm.rcv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time"
|
||||
|
||||
et = time.monotonic() - start_time
|
||||
assert et < 5, f"pigeond took {et:.1f}s to start"
|
||||
managed_processes['pigeond'].stop()
|
||||
|
||||
def test_turns_off_ublox(self):
|
||||
for s in (0.1, 0.5, 1, 5):
|
||||
managed_processes['pigeond'].start()
|
||||
time.sleep(s)
|
||||
managed_processes['pigeond'].stop()
|
||||
|
||||
assert gpio_read(GPIO.UBLOX_RST_N) == 0
|
||||
assert gpio_read(GPIO.UBLOX_PWR_EN) == 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Executable
+271
@@ -0,0 +1,271 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import unittest
|
||||
import numpy as np
|
||||
from collections import namedtuple, defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from system.hardware import TICI, HARDWARE
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
|
||||
BMX = {
|
||||
('bmx055', 'acceleration'),
|
||||
('bmx055', 'gyroUncalibrated'),
|
||||
('bmx055', 'magneticUncalibrated'),
|
||||
('bmx055', 'temperature'),
|
||||
}
|
||||
|
||||
LSM = {
|
||||
('lsm6ds3', 'acceleration'),
|
||||
('lsm6ds3', 'gyroUncalibrated'),
|
||||
('lsm6ds3', 'temperature'),
|
||||
}
|
||||
LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
|
||||
|
||||
MMC = {
|
||||
('mmc5603nj', 'magneticUncalibrated'),
|
||||
}
|
||||
|
||||
RPR = {
|
||||
('rpr0521', 'light'),
|
||||
}
|
||||
|
||||
SENSOR_CONFIGURATIONS = (
|
||||
(BMX | LSM | RPR),
|
||||
(MMC | LSM | RPR),
|
||||
(BMX | LSM_C | RPR),
|
||||
(MMC| LSM_C | RPR),
|
||||
)
|
||||
|
||||
Sensor = log.SensorEventData.SensorSource
|
||||
SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
|
||||
ALL_SENSORS = {
|
||||
Sensor.rpr0521: {
|
||||
SensorConfig("light", 0, 1023),
|
||||
},
|
||||
|
||||
Sensor.lsm6ds3: {
|
||||
SensorConfig("acceleration", 5, 15),
|
||||
SensorConfig("gyroUncalibrated", 0, .2),
|
||||
SensorConfig("temperature", 0, 60),
|
||||
},
|
||||
|
||||
Sensor.lsm6ds3trc: {
|
||||
SensorConfig("acceleration", 5, 15),
|
||||
SensorConfig("gyroUncalibrated", 0, .2),
|
||||
SensorConfig("temperature", 0, 60),
|
||||
},
|
||||
|
||||
Sensor.bmx055: {
|
||||
SensorConfig("acceleration", 5, 15),
|
||||
SensorConfig("gyroUncalibrated", 0, .2),
|
||||
SensorConfig("magneticUncalibrated", 0, 300),
|
||||
SensorConfig("temperature", 0, 60),
|
||||
},
|
||||
|
||||
Sensor.mmc5603nj: {
|
||||
SensorConfig("magneticUncalibrated", 0, 300),
|
||||
}
|
||||
}
|
||||
|
||||
LSM_IRQ = 336
|
||||
|
||||
def get_irq_count(irq: int):
|
||||
with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
|
||||
per_cpu = map(int, f.read().split(","))
|
||||
return sum(per_cpu)
|
||||
|
||||
def read_sensor_events(duration_sec):
|
||||
sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2',
|
||||
'gyroscope2', 'lightSensor', 'temperatureSensor']
|
||||
esocks = {}
|
||||
events = defaultdict(list)
|
||||
for stype in sensor_types:
|
||||
esocks[stype] = messaging.sub_sock(stype, timeout=0.1)
|
||||
|
||||
start_time_sec = time.monotonic()
|
||||
while time.monotonic() - start_time_sec < duration_sec:
|
||||
for esock in esocks:
|
||||
events[esock] += messaging.drain_sock(esocks[esock])
|
||||
time.sleep(0.1)
|
||||
|
||||
assert sum(map(len, events.values())) != 0, "No sensor events collected!"
|
||||
|
||||
return events
|
||||
|
||||
class TestSensord(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if not TICI:
|
||||
raise unittest.SkipTest
|
||||
|
||||
# make sure gpiochip0 is readable
|
||||
HARDWARE.initialize_hardware()
|
||||
|
||||
# enable LSM self test
|
||||
os.environ["LSM_SELF_TEST"] = "1"
|
||||
|
||||
# read initial sensor values every test case can use
|
||||
os.system("pkill -f ./_sensord")
|
||||
try:
|
||||
managed_processes["sensord"].start()
|
||||
time.sleep(3)
|
||||
cls.sample_secs = 10
|
||||
cls.events = read_sensor_events(cls.sample_secs)
|
||||
finally:
|
||||
# teardown won't run if this doesn't succeed
|
||||
managed_processes["sensord"].stop()
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
managed_processes["sensord"].stop()
|
||||
if "LSM_SELF_TEST" in os.environ:
|
||||
del os.environ['LSM_SELF_TEST']
|
||||
|
||||
def tearDown(self):
|
||||
managed_processes["sensord"].stop()
|
||||
|
||||
def test_sensors_present(self):
|
||||
# verify correct sensors configuration
|
||||
|
||||
seen = set()
|
||||
for etype in self.events:
|
||||
for measurement in self.events[etype]:
|
||||
m = getattr(measurement, measurement.which())
|
||||
seen.add((str(m.source), m.which()))
|
||||
|
||||
self.assertIn(seen, SENSOR_CONFIGURATIONS)
|
||||
|
||||
def test_lsm6ds3_timing(self):
|
||||
# verify measurements are sampled and published at 104Hz
|
||||
|
||||
sensor_t = {
|
||||
1: [], # accel
|
||||
5: [], # gyro
|
||||
}
|
||||
|
||||
for measurement in self.events['accelerometer']:
|
||||
m = getattr(measurement, measurement.which())
|
||||
sensor_t[m.sensor].append(m.timestamp)
|
||||
|
||||
for measurement in self.events['gyroscope']:
|
||||
m = getattr(measurement, measurement.which())
|
||||
sensor_t[m.sensor].append(m.timestamp)
|
||||
|
||||
for s, vals in sensor_t.items():
|
||||
with self.subTest(sensor=s):
|
||||
assert len(vals) > 0
|
||||
tdiffs = np.diff(vals) / 1e6 # millis
|
||||
|
||||
high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs))
|
||||
assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}"
|
||||
|
||||
avg_diff = sum(tdiffs)/len(tdiffs)
|
||||
avg_freq = 1. / (avg_diff * 1e-3)
|
||||
assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz"
|
||||
|
||||
stddev = np.std(tdiffs)
|
||||
assert stddev < 2.0, f"Standard-dev to big {stddev}"
|
||||
|
||||
def test_events_check(self):
|
||||
# verify if all sensors produce events
|
||||
|
||||
sensor_events = dict()
|
||||
for etype in self.events:
|
||||
for measurement in self.events[etype]:
|
||||
m = getattr(measurement, measurement.which())
|
||||
|
||||
if m.type in sensor_events:
|
||||
sensor_events[m.type] += 1
|
||||
else:
|
||||
sensor_events[m.type] = 1
|
||||
|
||||
for s in sensor_events:
|
||||
err_msg = f"Sensor {s}: 200 < {sensor_events[s]}"
|
||||
assert sensor_events[s] > 200, err_msg
|
||||
|
||||
def test_logmonottime_timestamp_diff(self):
|
||||
# ensure diff between the message logMonotime and sample timestamp is small
|
||||
|
||||
tdiffs = list()
|
||||
for etype in self.events:
|
||||
for measurement in self.events[etype]:
|
||||
m = getattr(measurement, measurement.which())
|
||||
|
||||
# check if gyro and accel timestamps are before logMonoTime
|
||||
if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature':
|
||||
err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
|
||||
assert m.timestamp < measurement.logMonoTime, err_msg
|
||||
|
||||
# negative values might occur, as non interrupt packages created
|
||||
# before the sensor is read
|
||||
tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
|
||||
|
||||
high_delay_diffs = set(filter(lambda d: d >= 15., tdiffs))
|
||||
assert len(high_delay_diffs) < 20, f"Too many measurements published : {high_delay_diffs}"
|
||||
|
||||
avg_diff = round(sum(tdiffs)/len(tdiffs), 4)
|
||||
assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
|
||||
|
||||
stddev = np.std(tdiffs)
|
||||
assert stddev < 2, f"Timing diffs have too high stddev: {stddev}"
|
||||
|
||||
def test_sensor_values_sanity_check(self):
|
||||
sensor_values = dict()
|
||||
for etype in self.events:
|
||||
for measurement in self.events[etype]:
|
||||
m = getattr(measurement, measurement.which())
|
||||
key = (m.source.raw, m.which())
|
||||
values = getattr(m, m.which())
|
||||
|
||||
if hasattr(values, 'v'):
|
||||
values = values.v
|
||||
values = np.atleast_1d(values)
|
||||
|
||||
if key in sensor_values:
|
||||
sensor_values[key].append(values)
|
||||
else:
|
||||
sensor_values[key] = [values]
|
||||
|
||||
# Sanity check sensor values and counts
|
||||
for sensor, stype in sensor_values:
|
||||
for s in ALL_SENSORS[sensor]:
|
||||
if s.type != stype:
|
||||
continue
|
||||
|
||||
key = (sensor, s.type)
|
||||
val_cnt = len(sensor_values[key])
|
||||
min_samples = self.sample_secs * 100 # Hz
|
||||
err_msg = f"Sensor {sensor} {s.type} got {val_cnt} measurements, expected {min_samples}"
|
||||
assert min_samples*0.9 < val_cnt < min_samples*1.1, err_msg
|
||||
|
||||
mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
|
||||
err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
|
||||
assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg
|
||||
|
||||
def test_sensor_verify_no_interrupts_after_stop(self):
|
||||
managed_processes["sensord"].start()
|
||||
time.sleep(3)
|
||||
|
||||
# read /proc/interrupts to verify interrupts are received
|
||||
state_one = get_irq_count(LSM_IRQ)
|
||||
time.sleep(1)
|
||||
state_two = get_irq_count(LSM_IRQ)
|
||||
|
||||
error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}"
|
||||
assert state_one != state_two, error_msg
|
||||
|
||||
managed_processes["sensord"].stop()
|
||||
time.sleep(1)
|
||||
|
||||
# read /proc/interrupts to verify no more interrupts are received
|
||||
state_one = get_irq_count(LSM_IRQ)
|
||||
time.sleep(1)
|
||||
state_two = get_irq_count(LSM_IRQ)
|
||||
assert state_one == state_two, "Interrupts received after sensord stop!"
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Executable
+48
@@ -0,0 +1,48 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import time
|
||||
import atexit
|
||||
|
||||
from cereal import messaging
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
|
||||
TIMEOUT = 10*60
|
||||
|
||||
def kill():
|
||||
for proc in ['ubloxd', 'pigeond']:
|
||||
managed_processes[proc].stop(retry=True, block=True)
|
||||
|
||||
if __name__ == "__main__":
|
||||
# start ubloxd
|
||||
managed_processes['ubloxd'].start()
|
||||
atexit.register(kill)
|
||||
|
||||
sm = messaging.SubMaster(['ubloxGnss'])
|
||||
|
||||
times = []
|
||||
for i in range(20):
|
||||
# start pigeond
|
||||
st = time.monotonic()
|
||||
managed_processes['pigeond'].start()
|
||||
|
||||
# wait for a >4 satellite fix
|
||||
while True:
|
||||
sm.update(0)
|
||||
msg = sm['ubloxGnss']
|
||||
if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
|
||||
report = msg.measurementReport
|
||||
if report.numMeas > 4:
|
||||
times.append(time.monotonic() - st)
|
||||
print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
|
||||
break
|
||||
|
||||
if time.monotonic() - st > TIMEOUT:
|
||||
raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
|
||||
|
||||
time.sleep(0.1)
|
||||
|
||||
# stop pigeond
|
||||
managed_processes['pigeond'].stop(retry=True, block=True)
|
||||
time.sleep(20)
|
||||
|
||||
print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")
|
||||
Reference in New Issue
Block a user