mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-01 03:43:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd no GPS (#33029)

Browse Source 
* Pose kf draft

old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b

* Fix it

old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b

* Add translation noise

old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d

* Add gravity to acc

old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613

* Use pyx

old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa

* Indent

old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67

* Reset function

old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc

* Add device_from_ned and ned_from_device transformations

old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae

* Fix rotations

old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6

* kms

old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2

* Centripetal acceleration

old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71

* Rewrite draft

old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c

* Remove old locationd stuff

old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba

* Python process now

old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3

* Process replay fix

old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9

* Add checks for timing and validity

old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36

* Fixes

old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267

* Process replay config fixes

old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e

* static analysis fixes

old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866

* lp in latcontrol

old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca

* Fix SensorEvent name for acceleration

old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd

* Ignore sensor readings from segments with multiple imus

old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0

* Update shebang

old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797

* Replace llk with lp

old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0

* Refactor locationd scenario test

old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84

* Add more debugging tools

old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1

* Param name update

old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337

* Fix expected observations

old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75

* Handle invalid measurements

old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf

* Fix spelling

old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b

* Include observations in debug info too

old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868

* Store error instead of expected observation

old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c

* Renames

old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d

* Zero the yaw

old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179

* New state_dot for orientation

old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7

* Fix the state transformations

old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db

* Update process in test_onroad

old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617

* Test polling on cameraOdometry

old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8

* Keep the copy of x and P returned from predict

old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd

* Remove polling again

old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68

* Remove locationd.cc

old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663

* Optim in _finite_check

old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5

* Access .t once

old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399

* Move the timing check to cam odo code path

old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e

* Call all_checks only once

old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78

* Do not sort

old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025

* Check sm.updated

old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4

* Remove test_params_gps

old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858

* Increase tolerance

old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3

* Fix static

old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490

* Try separate sockets for sensors

old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1

* sensor_all_checks

old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3

* Fix static

old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e

* Set the cpu limit to 25

old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d

* Make it prettier

old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7

* Prettier

old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5

* Increase the cpu budget to 260

old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4

* Change trans std mult. 2 works better

* Update ref commit

* Update ref commit
This commit is contained in:
Kacper Rączy
2024-09-04 02:54:57 -07:00
committed by GitHub
parent 0058ea5817
commit 236dffe400
15 changed files with 484 additions and 1285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/test/test_locationd.py
-33
View File
@@ -1,13 +1,7 @@
import pytest
import json
import random
import time
import capnp
import cereal.messaging as messaging
from cereal.services import SERVICE_LIST
from openpilot.common.params import Params
from openpilot.common.transformations.coordinates import ecef2geodetic
from openpilot.system.manager.process_config import managed_processes
@@ -60,30 +54,3 @@ class TestLocationdProc:
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.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])
# get fake messages at the correct frequency, listed in services.py
msgs = []
for sec in range(65):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
time.sleep(1) # wait for async params write
lastGPS = json.loads(self.params.get('LastGPSPosition'))
assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)
selfdrive/locationd/test/test_locationd_scenarios.py
+3 -70
View File
@@ -21,10 +21,6 @@ JUNK_IDX = 100
class Scenario(Enum):
BASE = 'base'
GPS_OFF = 'gps_off'
GPS_OFF_MIDWAY = 'gps_off_midway'
GPS_ON_MIDWAY = 'gps_on_midway'
GPS_TUNNEL = 'gps_tunnel'
GYRO_OFF = 'gyro_off'
GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
ACCEL_OFF = 'accel_off'
@@ -51,24 +47,6 @@ def run_scenarios(scenario, logs):
if scenario == Scenario.BASE:
pass
elif scenario == Scenario.GPS_OFF:
logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_OFF_MIDWAY:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_ON_MIDWAY:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_TUNNEL:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GYRO_OFF:
logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
@@ -116,52 +94,7 @@ class TestLocationdScenarios:
- roll: unchanged
"""
orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_off(self):
"""
Test: no GPS message for the entire segment
Expected Result:
- yaw_rate: unchanged
- roll:
- gpsOK: False
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_off_midway(self):
"""
Test: no GPS message for the second half of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_on_midway(self):
"""
Test: no GPS message for the first half of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))
def test_gps_tunnel(self):
"""
Test: no GPS message for the middle section of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gyro_off(self):
@@ -186,7 +119,7 @@ class TestLocationdScenarios:
- inputsOK: False for some time after the spike, True for the rest
"""
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
assert np.diff(replayed_data['inputs_flag'])[696] == 1.0
@@ -211,5 +144,5 @@ class TestLocationdScenarios:
Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
"""
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))