mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-12 21:02:13 +08:00
Kurt Nistelberger
9570336e6c
* laikad update, renaming * update locationd * fix naming * address PR comments * upsi * . * draft to fix replay * fix process relay to allow no response for messages * final fix for process replay * . * bump cereal * update process replay ref commit * reduce wait time * . * last ref change * move laikad helpers to laika * . * fix ublox test * update refs * add proper qcom replay support * fix gnss support if both is available * update refs * remove left over * revert laikad msg * move laika back to master * init * fix gps valid flag * change time * add gnss to ignore * remove gps_valid flag * . * adopt orientation reset threshold * . * update laikad * . * fix stanstill KF resets * test orienation reset count * update laika * bump cereal * fix process replay * update laika repo * remove handle gps * add extra logging for cache * . * add more log * . * . * update laika * dont remove gps code * inc min satellite count * update magic vals and add acc drop * update laika * upsi * rem * bump laika * use nav and correct * more fixes * use sftp * No more glonass * Revert "No more glonass" This reverts commit a76124da50a1e25f423ad1137c7a046e1d57811d. * nump laika * back support old ephemeris cache * add health to ephemeris message * bump laika * remove print * fix laikad tests * clean * remove extra log * bump laika * inc timeout for plotjuggler build * rem cache clear * . * enable gps after checks Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com> Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com> Co-authored-by: Shane Smiskol <shane@smiskol.com> old-commit-hash: 88423e25df8bfbd8e6a3275eb086f468cf9a59f1
94 lines
3.0 KiB
Python
Executable File
94 lines
3.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import json
|
|
import random
|
|
import unittest
|
|
import time
|
|
import capnp
|
|
|
|
import cereal.messaging as messaging
|
|
from cereal.services import service_list
|
|
from common.params import Params
|
|
from common.transformations.coordinates import ecef2geodetic
|
|
|
|
from selfdrive.manager.process_config import managed_processes
|
|
|
|
|
|
class TestLocationdProc(unittest.TestCase):
|
|
MAX_WAITS = 1000
|
|
LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
|
|
'accelerometer', 'gyroscope', 'magnetometer']
|
|
|
|
def setUp(self):
|
|
random.seed(123489234)
|
|
|
|
self.pm = messaging.PubMaster(self.LLD_MSGS)
|
|
|
|
Params().put_bool("UbloxAvailable", True)
|
|
managed_processes['locationd'].prepare()
|
|
managed_processes['locationd'].start()
|
|
|
|
time.sleep(1)
|
|
|
|
def tearDown(self):
|
|
managed_processes['locationd'].stop()
|
|
|
|
def send_msg(self, msg):
|
|
self.pm.send(msg.which(), msg)
|
|
waits_left = self.MAX_WAITS
|
|
while waits_left and not self.pm.all_readers_updated(msg.which()):
|
|
time.sleep(0)
|
|
waits_left -= 1
|
|
time.sleep(0.0001)
|
|
|
|
def get_fake_msg(self, name, t):
|
|
try:
|
|
msg = messaging.new_message(name)
|
|
except capnp.lib.capnp.KjException:
|
|
msg = messaging.new_message(name, 0)
|
|
|
|
if name == "gnssMeasurements":
|
|
msg.gnssMeasurements.measTime = t
|
|
msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
|
|
msg.gnssMeasurements.positionECEF.std = [0,0,0]
|
|
msg.gnssMeasurements.positionECEF.valid = True
|
|
msg.gnssMeasurements.velocityECEF.value = []
|
|
msg.gnssMeasurements.velocityECEF.std = [0,0,0]
|
|
msg.gnssMeasurements.velocityECEF.valid = True
|
|
elif name == 'cameraOdometry':
|
|
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
|
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
|
|
msg.logMonoTime = t
|
|
return msg
|
|
|
|
def test_params_gps(self):
|
|
# first reset params
|
|
Params().remove('LastGPSPosition')
|
|
|
|
self.x = -2710700 + (random.random() * 1e5)
|
|
self.y = -4280600 + (random.random() * 1e5)
|
|
self.z = 3850300 + (random.random() * 1e5)
|
|
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
|
|
|
|
self.fake_duration = 90 # secs
|
|
# get fake messages at the correct frequency, listed in services.py
|
|
fake_msgs = []
|
|
for sec in range(self.fake_duration):
|
|
for name in self.LLD_MSGS:
|
|
for j in range(int(service_list[name].frequency)):
|
|
fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9)))
|
|
|
|
for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime):
|
|
self.send_msg(fake_msg)
|
|
time.sleep(1) # wait for async params write
|
|
|
|
lastGPS = json.loads(Params().get('LastGPSPosition'))
|
|
self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4)
|
|
self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4)
|
|
self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|