You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
77 lines
1.9 KiB
77 lines
1.9 KiB
5 months ago
|
import time
|
||
|
|
|
||
|
|
from cereal import log
|
||
|
|
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
|
||
|
|
|
||
|
|
# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf
|
||
|
|
|
||
|
|
# Register addresses
|
||
|
|
REG_ODR = 0x1A
|
||
|
|
REG_INTERNAL_0 = 0x1B
|
||
|
|
REG_INTERNAL_1 = 0x1C
|
||
|
|
|
||
|
|
# Control register settings
|
||
|
|
CMM_FREQ_EN = (1 << 7)
|
||
|
|
AUTO_SR_EN = (1 << 5)
|
||
|
|
SET = (1 << 3)
|
||
|
|
RESET = (1 << 4)
|
||
|
|
|
||
|
|
class MMC5603NJ_Magn(Sensor):
|
||
|
|
@property
|
||
|
|
def device_address(self) -> int:
|
||
|
|
return 0x30
|
||
|
|
|
||
|
|
def init(self):
|
||
|
|
self.verify_chip_id(0x39, [0x10, ])
|
||
|
|
self.writes((
|
||
|
|
(REG_ODR, 0),
|
||
|
|
|
||
|
|
# Set BW to 0b01 for 1-150 Hz operation
|
||
|
|
(REG_INTERNAL_1, 0b01),
|
||
|
|
))
|
||
|
|
|
||
|
|
def _read_data(self, cycle) -> list[float]:
|
||
|
|
# start measurement
|
||
|
|
self.write(REG_INTERNAL_0, cycle)
|
||
|
|
self.wait()
|
||
|
|
|
||
|
|
# read out XYZ
|
||
|
|
scale = 1.0 / 16384.0
|
||
|
|
b = self.read(0x00, 9)
|
||
|
|
return [
|
||
|
|
(self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0,
|
||
|
|
(self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0,
|
||
|
|
(self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0,
|
||
|
|
]
|
||
|
|
|
||
|
|
def get_event(self, ts: int | None = None) -> log.SensorEventData:
|
||
|
|
ts = time.monotonic_ns()
|
||
|
|
|
||
|
|
# SET - RESET cycle
|
||
|
|
xyz = self._read_data(SET)
|
||
|
|
reset_xyz = self._read_data(RESET)
|
||
|
|
vals = [*xyz, *reset_xyz]
|
||
|
|
|
||
|
|
event = log.SensorEventData.new_message()
|
||
|
|
event.timestamp = ts
|
||
|
|
event.version = 1
|
||
|
|
event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED
|
||
|
|
event.type = 14 # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED
|
||
|
|
event.source = log.SensorEventData.SensorSource.mmc5603nj
|
||
|
|
|
||
|
|
m = event.init('magneticUncalibrated')
|
||
|
|
m.v = vals
|
||
|
|
m.status = int(all(int(v) != -32 for v in vals))
|
||
|
|
|
||
|
|
return event
|
||
|
|
|
||
|
|
def shutdown(self) -> None:
|
||
|
|
v = self.read(REG_INTERNAL_0, 1)[0]
|
||
|
|
self.writes((
|
||
|
|
# disable auto-reset of measurements
|
||
|
|
(REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))),
|
||
|
|
|
||
|
|
# disable continuous mode
|
||
|
|
(REG_ODR, 0),
|
||
|
|
))
|