mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-21 22:23:53 +08:00
29d3ed2ce6
* PoC of reading sensors via interrupts instead of polling * add Gyro and draft for magn * add more functionality to gpio.cc * change LSM gyro to interrupt * resolve rebase conflict * update BMX accel interrupt impl * add interrupt collector thread to fetch in parallel * change get_event interface to return true on successful read * update BMX gyro interrupt impl * update gpio.h/.cc according to comments * address comments, rename Edgetype enum * Edgetype to EdgeType * update sensor interrupt interface * add error handling, and read fd on trigger * avoid sending empty messages * fix build * use gpiochip * less diff * gpiochip on both edges, but skip falling edge if rising edge is detected * init last_ts with 0 * update sensord testcases * update sensord testsweet * test for pipeline * readd with_process * add null check * move tests update to seperate PR * sensord: improve test coverage (#25683) * update sensord-interrupt testsweet * address review comments * inc stddev threshold * fix format string * add version 0 check again * relax strictness after c3 with bmx tests * relax strictness after tests Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com> * address PR comments * fix typo * remove 4ms limit, and skip first 0.5sec of data * revert disable_interuppt change to destructor * fix and remove timing skip * make gpiochip generic * sensord port * change from sensorEvents to separated events * fix gyro usage * add splitted sensor tests * modify debug script sensor_data_to_hist.py * refactor get_event interface to remove sensorEvent message type * update locationd to non sensorEvent usage * tmp commit * fix replay * fix accelerometer type * fix sensor to hist debug script * update sensord tests to split events * remove rebase artifacts * port test_sensord.py * small clean up * change cereal to sensorEvents-splitup branch * upate sensorEvents in regen * fix route generation for splitted sensor events * regen cleanUp from sensorEvents change * . * remove light and temp from locationd * add generic init delay per sensor * . * update routes * move bmx gyro/accel to its own channel * adopt sensor tests to bmx channel * remove rebase artifacts * fix sensord test * handle bmx not present * add bmx sockets to regen * . * . * code cleanUp * . * address PR comments * address PR comments * address PR comments * lsm clean up * readd sensorEvents * rever regen.py * . * update replay refs * move channels * fix artifact * bump cereal * update refs * fix timing issue Co-authored-by: Bruce Wayne <batman@workstation-eu-intern2.eu.local> Co-authored-by: gast04 <kurt.nistelberger@gmail.com> Co-authored-by: Willem Melching <willem.melching@gmail.com> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
107 lines
3.9 KiB
Python
Executable File
107 lines
3.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""This test can't be run together with other locationd tests.
|
|
cffi.dlopen breaks the list of registered filters."""
|
|
import os
|
|
import random
|
|
import unittest
|
|
|
|
from cffi import FFI
|
|
|
|
import cereal.messaging as messaging
|
|
from cereal import log
|
|
|
|
SENSOR_DECIMATION = 1
|
|
VISION_DECIMATION = 1
|
|
|
|
LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so'))
|
|
|
|
|
|
class TestLocationdLib(unittest.TestCase):
|
|
def setUp(self):
|
|
header = '''typedef ...* Localizer_t;
|
|
Localizer_t localizer_init();
|
|
void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
|
|
void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''
|
|
|
|
self.ffi = FFI()
|
|
self.ffi.cdef(header)
|
|
self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH)
|
|
|
|
self.localizer = self.lib.localizer_init()
|
|
|
|
self.buff_size = 2048
|
|
self.msg_buff = self.ffi.new(f'char[{self.buff_size}]')
|
|
|
|
def localizer_handle_msg(self, msg_builder):
|
|
bytstr = msg_builder.to_bytes()
|
|
self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))
|
|
|
|
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
|
|
self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
|
|
return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)
|
|
|
|
def test_liblocalizer(self):
|
|
msg = messaging.new_message('liveCalibration')
|
|
msg.liveCalibration.validBlocks = random.randint(1, 10)
|
|
msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)]
|
|
|
|
self.localizer_handle_msg(msg)
|
|
liveloc = self.localizer_get_msg()
|
|
self.assertTrue(liveloc is not None)
|
|
|
|
@unittest.skip("temporarily disabled due to false positives")
|
|
def test_device_fell(self):
|
|
msg = messaging.new_message('accelerometer')
|
|
msg.accelerometer.sensor = 1
|
|
msg.accelerometer.timestamp = msg.logMonoTime
|
|
msg.accelerometer.type = 1
|
|
msg.accelerometer.init('acceleration')
|
|
msg.accelerometer.acceleration.v = [10.0, 0.0, 0.0] # zero with gravity
|
|
self.localizer_handle_msg(msg)
|
|
|
|
ret = self.localizer_get_msg()
|
|
self.assertTrue(ret.liveLocationKalman.deviceStable)
|
|
|
|
msg = messaging.new_message('accelerometer')
|
|
msg.accelerometer.sensor = 1
|
|
msg.accelerometer.timestamp = msg.logMonoTime
|
|
msg.accelerometer.type = 1
|
|
msg.accelerometer.init('acceleration')
|
|
msg.accelerometer.acceleration.v = [50.1, 0.0, 0.0] # more than 40 m/s**2
|
|
self.localizer_handle_msg(msg)
|
|
|
|
ret = self.localizer_get_msg()
|
|
self.assertFalse(ret.liveLocationKalman.deviceStable)
|
|
|
|
def test_posenet_spike(self):
|
|
for _ in range(SENSOR_DECIMATION):
|
|
msg = messaging.new_message('carState')
|
|
msg.carState.vEgo = 6.0 # more than 5 m/s
|
|
self.localizer_handle_msg(msg)
|
|
|
|
ret = self.localizer_get_msg()
|
|
self.assertTrue(ret.liveLocationKalman.posenetOK)
|
|
|
|
for _ in range(20 * VISION_DECIMATION): # size of hist_old
|
|
msg = messaging.new_message('cameraOdometry')
|
|
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1]
|
|
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.transStd = [2.0, 0.1, 0.1]
|
|
self.localizer_handle_msg(msg)
|
|
|
|
for _ in range(20 * VISION_DECIMATION): # size of hist_new
|
|
msg = messaging.new_message('cameraOdometry')
|
|
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0]
|
|
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger
|
|
self.localizer_handle_msg(msg)
|
|
|
|
ret = self.localizer_get_msg()
|
|
self.assertFalse(ret.liveLocationKalman.posenetOK)
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|
|
|