diff --git a/cereal b/cereal index 9c0c517bc8..35979b7e2e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 9c0c517bc8b136991b21a759fee743228eb680ba +Subproject commit 35979b7e2e52b2e743b2609d11365508760df63b diff --git a/docs/CARS.md b/docs/CARS.md index 5100a8306d..87be32f1d0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -150,7 +150,7 @@ How We Rate The Cars |Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| |Volkswagen|Jetta 2018-21|Driver Assistance|||||| |Volkswagen|Jetta GLI 2021|Driver Assistance|||||| -|Volkswagen|Passat 2016-18[7](#footnotes)|Driver Assistance|||||| +|Volkswagen|Passat 2015-18[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Polo 2020|Driver Assistance|||||| |Volkswagen|T-Cross 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|T-Roc 2021[8](#footnotes)|Driver Assistance|||||| diff --git a/laika_repo b/laika_repo index f5f76d28b4..231eafbf65 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd +Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e diff --git a/release/files_common b/release/files_common index 1b75fb9a11..e254abec66 100644 --- a/release/files_common +++ b/release/files_common @@ -168,7 +168,6 @@ selfdrive/controls/lib/events.py selfdrive/controls/lib/lane_planner.py selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py -selfdrive/controls/lib/latcontrol_lqr.py selfdrive/controls/lib/latcontrol_torque.py selfdrive/controls/lib/latcontrol_pid.py selfdrive/controls/lib/latcontrol.py diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 2e8f7093da..db1342ee7d 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -7,7 +7,7 @@ from PIL import Image from typing import List import cereal.messaging as messaging -from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcClient, VisionStreamType from common.params import Params from common.realtime import DT_MDL from selfdrive.hardware import TICI, PC diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 452e782ee4..b4dc26c87a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -7,6 +7,7 @@ from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -48,7 +49,7 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiV = [0.0] ret.stopAccel = 0.0 - ret.longitudinalActuatorDelayUpperBound = 1.0 # s + ret.longitudinalActuatorDelayUpperBound = 1.0 # s if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022): ret.lateralTuning.pid.kf = 0.00005 @@ -252,12 +253,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.65 max_lat_accel = 2. - ret.lateralTuning.init('torque') - ret.lateralTuning.torque.useSteeringAngle = True - ret.lateralTuning.torque.kp = 1.0 / max_lat_accel - ret.lateralTuning.torque.kf = 1.0 / max_lat_accel - ret.lateralTuning.torque.ki = 0.1 / max_lat_accel - ret.lateralTuning.torque.friction = 0.01 + set_torque_tune(ret.lateralTuning, max_lat_accel, 0.01) # Genesis elif candidate == CAR.GENESIS_G70: diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 18bf9c1f01..92024ab0c2 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -38,8 +38,6 @@ class TestCarInterfaces(unittest.TestCase): tuning = car_params.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(car_params.lateralTuning.pid.kpV)) - elif tuning == 'lqr': - self.assertTrue(len(car_params.lateralTuning.lqr.a)) elif tuning == 'torque': self.assertTrue(car_params.lateralTuning.torque.kf > 0) elif tuning == 'indi': diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 27669fd331..d2867b9f4f 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -118,8 +118,6 @@ class TestCarModel(unittest.TestCase): self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) elif tuning == 'torque': self.assertTrue(self.CP.lateralTuning.torque.kf > 0) - elif tuning == 'lqr': - self.assertTrue(len(self.CP.lateralTuning.lqr.a)) elif tuning == 'indi': self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) else: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index fd3bd15cf1..d8e642ba61 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -55,7 +55,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 @@ -87,7 +87,10 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05) + else: + set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): stop_and_go = True @@ -136,7 +139,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index c200e39c0c..2cfdd31644 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from enum import Enum - +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune class LongTunes(Enum): PEDAL = 0 @@ -52,22 +52,7 @@ def set_long_tune(tune, name): ###### LAT ###### def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1): if name == LatTunes.TORQUE: - tune.init('torque') - tune.torque.useSteeringAngle = True - tune.torque.kp = 1.0 / MAX_LAT_ACCEL - tune.torque.kf = 1.0 / MAX_LAT_ACCEL - tune.torque.ki = 0.1 / MAX_LAT_ACCEL - tune.torque.friction = FRICTION - elif name == LatTunes.LQR_RAV4: - tune.init('lqr') - tune.lqr.scale = 1500.0 - tune.lqr.ki = 0.05 - tune.lqr.a = [0., 1., -0.22619643, 1.21822268] - tune.lqr.b = [-1.92006585e-04, 3.95603032e-05] - tune.lqr.c = [1., 0.] - tune.lqr.k = [-110.73572306, 451.22718255] - tune.lqr.l = [0.3233671, 0.3185757] - tune.lqr.dcGain = 0.002237852961363602 + set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) elif name == LatTunes.INDI_PRIUS: tune.init('indi') tune.indi.innerLoopGainBP = [0.] diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 1dc431eb77..6367dd8969 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -135,7 +135,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Jetta 2018-21"), VWCarInfo("Volkswagen Jetta GLI 2021"), ], - CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2016-18", footnotes=[Footnote.PASSAT]), + CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2015-18", footnotes=[Footnote.PASSAT]), CAR.POLO_MK6: VWCarInfo("Volkswagen Polo 2020"), CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), @@ -439,6 +439,7 @@ FW_VERSIONS = { CAR.PASSAT_MK8: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906023AH\xf1\x893379', + b'\xf1\x8704L906026ET\xf1\x891990', b'\xf1\x8704L906026GA\xf1\x892013', b'\xf1\x8704L906026KD\xf1\x894798', b'\xf1\x873G0906264 \xf1\x890004', @@ -446,22 +447,26 @@ FW_VERSIONS = { (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300048R \xf1\x890610', b'\xf1\x870D9300014L \xf1\x895002', + b'\xf1\x870D9300041A \xf1\x894801', b'\xf1\x870DD300045T \xf1\x891601', b'\xf1\x870GC300042H \xf1\x891404', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311', b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211', b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x873Q0907572A \xf1\x890130', b'\xf1\x873Q0907572B \xf1\x890192', b'\xf1\x873Q0907572C \xf1\x890195', b'\xf1\x875Q0907572R \xf1\x890771', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5cb6874ca7..fc0f289e39 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -23,7 +23,6 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.latcontrol_torque import LatControlTorque -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -146,8 +145,6 @@ class Controls: self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP, self.CI) - elif self.CP.lateralTuning.which() == 'lqr': - self.LaC = LatControlLQR(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) @@ -752,8 +749,6 @@ class Controls: controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': controlsState.lateralControlState.torqueState = lac_log - elif lat_tuning == 'lqr': - controlsState.lateralControlState.lqrState = lac_log elif lat_tuning == 'indi': controlsState.lateralControlState.indiState = lac_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index 5b4f65a03c..0000000000 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ /dev/null @@ -1,84 +0,0 @@ -import math -import numpy as np - -from common.numpy_fast import clip -from common.realtime import DT_CTRL -from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED - - -class LatControlLQR(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) - self.scale = CP.lateralTuning.lqr.scale - self.ki = CP.lateralTuning.lqr.ki - - self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) - self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) - self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) - self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) - self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) - self.dc_gain = CP.lateralTuning.lqr.dcGain - - self.x_hat = np.array([[0], [0]]) - self.i_unwind_rate = 0.3 * DT_CTRL - self.i_rate = 1.0 * DT_CTRL - - self.reset() - - def reset(self): - super().reset() - self.i_lqr = 0.0 - - def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): - lqr_log = log.ControlsState.LateralLQRState.new_message() - - torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed - - # Subtract offset. Zero angle should correspond to zero torque - steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg - - desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) - - instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg - desired_angle += instant_offset # Only add offset that originates from vehicle model errors - lqr_log.steeringAngleDesiredDeg = desired_angle - - # Update Kalman filter - angle_steers_k = float(self.C.dot(self.x_hat)) - e = steering_angle_no_offset - angle_steers_k - self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) - - if CS.vEgo < MIN_STEER_SPEED or not active: - lqr_log.active = False - lqr_output = 0. - output_steer = 0. - self.reset() - else: - lqr_log.active = True - - # LQR - u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) - lqr_output = torque_scale * u_lqr / self.scale - - # Integrator - if CS.steeringPressed: - self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) - else: - error = desired_angle - angle_steers_k - i = self.i_lqr + self.ki * self.i_rate * error - control = lqr_output + i - - if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ - (error <= 0 and (control >= -self.steer_max or i > 0.0)): - self.i_lqr = i - - output_steer = lqr_output + self.i_lqr - output_steer = clip(output_steer, -self.steer_max, self.steer_max) - - lqr_log.steeringAngleDeg = angle_steers_k - lqr_log.i = self.i_lqr - lqr_log.output = output_steer - lqr_log.lqrOutput = lqr_output - lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) - return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e3dbe373b6..502d0abf0e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -22,6 +22,15 @@ LOW_SPEED_FACTOR = 200 JERK_THRESHOLD = 0.2 +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1): + tune.init('torque') + tune.torque.useSteeringAngle = True + tune.torque.kp = 1.0 / MAX_LAT_ACCEL + tune.torque.kf = 1.0 / MAX_LAT_ACCEL + tune.torque.ki = 0.1 / MAX_LAT_ACCEL + tune.torque.friction = FRICTION + + class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 8345840eca..503eaaa6a4 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.car.nissan.values import CAR as NISSAN from selfdrive.controls.lib.latcontrol_pid import LatControlPID -from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel class TestLatControl(unittest.TestCase): - @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) def test_saturation(self, car_name, controller): CarInterface, CarController, CarState = interfaces[car_name] CP = CarInterface.get_params(car_name) diff --git a/selfdrive/hardware/tici/restart_modem.sh b/selfdrive/hardware/tici/restart_modem.sh index 2fa5e60619..26c04e4fba 100755 --- a/selfdrive/hardware/tici/restart_modem.sh +++ b/selfdrive/hardware/tici/restart_modem.sh @@ -1,18 +1,18 @@ #!/usr/bin/bash -nmcli connection modify --temporary lte gsm.home-only yes -nmcli connection modify --temporary lte gsm.auto-config yes -nmcli connection modify --temporary lte connection.autoconnect-retries 20 +#nmcli connection modify --temporary lte gsm.home-only yes +#nmcli connection modify --temporary lte gsm.auto-config yes +#nmcli connection modify --temporary lte connection.autoconnect-retries 20 +sudo nmcli connection reload sudo systemctl stop ModemManager - -# full restart -#/usr/comma/lte/lte.sh stop_blocking -#sudo systemctl restart lte - -# quick shutdown -/usr/comma/lte/lte.sh stop nmcli con down lte +nmcli con down magenta-prime + +# power cycle modem +/usr/comma/lte/lte.sh stop_blocking +/usr/comma/lte/lte.sh start +sudo systemctl restart NetworkManager #sudo systemctl restart ModemManager sudo ModemManager --debug diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index d07fe4a78a..dae359b905 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -4,8 +4,11 @@ from typing import List import numpy as np from collections import defaultdict +from numpy.linalg import linalg + from cereal import log, messaging from laika import AstroDog +from laika.ephemeris import convert_ublox_ephem from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind @@ -19,28 +22,25 @@ MAX_TIME_GAP = 10 class Laikad: - def __init__(self): + def __init__(self, use_internet): + self.astro_dog = AstroDog(use_internet=use_internet) self.gnss_kf = GNSSKalman(GENERATED_DIR) - def process_ublox_msg(self, ublox_msg, dog: AstroDog, ublox_mono_time: int): + def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport new_meas = read_raw_ublox(report) - measurements = process_measurements(new_meas, dog) - - - pos_fix = calc_pos_fix(measurements) + measurements = process_measurements(new_meas, self.astro_dog) + pos_fix = calc_pos_fix(measurements, min_measurements=4) # To get a position fix a minimum of 5 measurements are needed. - # Each report can contain less and some measurement can't be processed. - if len(pos_fix) > 0: - measurements = correct_measurements(measurements, pos_fix[0][:3], dog) - meas_msgs = [create_measurement_msg(m) for m in measurements] + # Each report can contain less and some measurements can't be processed. + corrected_measurements = [] + if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100: + corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) t = ublox_mono_time * 1e-9 - - self.update_localizer(pos_fix, t, measurements) + self.update_localizer(pos_fix, t, corrected_measurements) localizer_valid = self.localizer_valid(t) - ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() @@ -49,6 +49,8 @@ class Laikad: bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) + meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] + dat = messaging.new_message("gnssMeasurements") measurement_msg = log.GnssMeasurements.Measurement.new_message dat.gnssMeasurements = { @@ -59,6 +61,11 @@ class Laikad: "correctedMeasurements": meas_msgs } return dat + elif ublox_msg.which == 'ephemeris': + ephem = convert_ublox_ephem(ublox_msg.ephemeris) + self.astro_dog.add_ephem(ephem, self.astro_dog.orbits) + # elif ublox_msg.which == 'ionoData': + # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid @@ -67,9 +74,10 @@ class Laikad: if len(pos_fix) == 0: return post_est = pos_fix[0][:3].tolist() - if self.gnss_kf.filter.filter_time is None: + filter_time = self.gnss_kf.filter.filter_time + if filter_time is None: cloudlog.info("Init gnss kalman filter") - elif (self.gnss_kf.filter.filter_time - t) > MAX_TIME_GAP: + elif (t - filter_time) > MAX_TIME_GAP: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") else: cloudlog.error("Gnss kalman filter state is nan") @@ -82,7 +90,7 @@ class Laikad: def localizer_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (filter_time - t) < MAX_TIME_GAP and \ + return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \ all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) def init_gnss_localizer(self, est_pos): @@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement): c = log.GnssMeasurements.CorrectedMeasurement.new_message() c.constellationId = meas.constellation_id.value c.svId = meas.sv_id - observables = meas.observables_final c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0 - c.pseudorange = float(observables['C1C']) + c.pseudorange = float(meas.observables_final['C1C']) c.pseudorangeStd = float(meas.observables_std['C1C']) - c.pseudorangeRate = float(observables['D1C']) + c.pseudorangeRate = float(meas.observables_final['D1C']) c.pseudorangeRateStd = float(meas.observables_std['D1C']) c.satPos = meas.sat_pos_final.tolist() c.satVel = meas.sat_vel.tolist() @@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): def main(): - dog = AstroDog(use_internet=True) sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) - laikad = Laikad() + laikad = Laikad(use_internet=True) while True: sm.update() - # Todo if no internet available use latest ephemeris if sm.updated['ubloxGnss']: ublox_msg = sm['ubloxGnss'] - msg = laikad.process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss']) + msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 3d94399e41..bf984becb8 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -2,7 +2,6 @@ import unittest from datetime import datetime -from laika import AstroDog from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement @@ -18,11 +17,11 @@ def get_log(segs=range(0)): return [m for m in logs if m.which() == 'ubloxGnss'] -def verify_messages(lr, dog, laikad): +def process_msgs(lr, laikad: Laikad): good_msgs = [] for m in lr: - msg = laikad.process_ublox_msg(m.ubloxGnss, dog, m.logMonoTime) - if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: + msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime) + if msg is not None: good_msgs.append(msg) return good_msgs @@ -44,14 +43,31 @@ class TestLaikad(unittest.TestCase): def test_laika_online(self): # Set to offline forces to use ephemeris messages - dog = AstroDog(use_internet=True) - laikad = Laikad() - correct_msgs = verify_messages(self.logs, dog, laikad) - + laikad = Laikad(use_internet=True) + msgs = process_msgs(self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + def test_laika_offline(self): + # Set to offline forces to use ephemeris messages + laikad = Laikad(use_internet=False) + msgs = process_msgs(self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + + self.assertEqual(256, len(correct_msgs)) + self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + + def test_laika_offline_ephem_at_start(self): + # Test offline but process ephemeris msgs of segment first + laikad = Laikad(use_internet=False) + ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris'] + msgs = process_msgs(ephemeris_logs+self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + self.assertEqual(554, len(correct_msgs)) + self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 1d3fcfce09..89fb0c5838 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -20,7 +20,7 @@ from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes from selfdrive.version import get_version from tools.lib.logreader import LogReader -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \ eon_d_frame_size, tici_d_frame_size, tici_e_frame_size diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py index f09aec578c..2fcff785a9 100755 --- a/selfdrive/modeld/test/test_modeld.py +++ b/selfdrive/modeld/test/test_modeld.py @@ -5,7 +5,7 @@ import numpy as np import random import cereal.messaging as messaging -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType from common.transformations.camera import tici_f_frame_size from common.realtime import DT_MDL from selfdrive.manager.process_config import managed_processes diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index ebbf5f72af..20739ae37d 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -7,7 +7,7 @@ from typing import Any from itertools import zip_longest import cereal.messaging as messaging -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType from common.spinner import Spinner from common.timeout import Timeout from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \ diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 276905a432..55ce9832cc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b8c35486e8354713221d4237e97e5abced6f5228 +336d77ad17b90af17b7eb24cc832e80b62d05a24 diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 3ceb989223..b0bc033fe6 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -11,7 +11,7 @@ os.environ["USE_WEBCAM"] = "1" import cereal.messaging as messaging from cereal import car from cereal.services import service_list -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType from common.params import Params from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size @@ -30,9 +30,17 @@ def replay_panda_states(s, msgs): rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']] + # TODO: new safety params from flags, remove after getting new routes for Toyota + safety_param_migration = { + "TOYOTA PRIUS 2017": 578, + "TOYOTA RAV4 2017": 329 + } + # Migrate safety param base on carState cp = [m for m in msgs if m.which() == 'carParams'][0].carParams - if len(cp.safetyConfigs): + if cp.carFingerprint in safety_param_migration: + safety_param = safety_param_migration[cp.carFingerprint] + elif len(cp.safetyConfigs): safety_param = cp.safetyConfigs[0].safetyParam if cp.safetyConfigs[0].safetyParamDEPRECATED != 0: safety_param = cp.safetyConfigs[0].safetyParamDEPRECATED @@ -181,15 +189,17 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): os.environ['SKIP_FW_QUERY'] = "" os.environ['FINGERPRINT'] = "" - # TODO: remove after getting new route for mazda - migration = { + # TODO: remove after getting new route for Mazda + fp_migration = { "Mazda CX-9 2021": "MAZDA CX-9 2021", } + # TODO: remove after getting new route for Subaru + fingerprint_problem = ["SUBARU IMPREZA LIMITED 2019"] for msg in lr: if msg.which() == 'carParams': - car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) - if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): + car_fingerprint = fp_migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) + if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS) and (car_fingerprint not in fingerprint_problem): params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) else: os.environ['SKIP_FW_QUERY'] = "1" diff --git a/tools/camerastream/compressed_vipc.py b/tools/camerastream/compressed_vipc.py index e6b0a70867..290610e45f 100755 --- a/tools/camerastream/compressed_vipc.py +++ b/tools/camerastream/compressed_vipc.py @@ -8,7 +8,7 @@ import multiprocessing import time import cereal.messaging as messaging -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType W, H = 1928, 1208 V4L2_BUF_FLAG_KEYFRAME = 8 diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 729d4cd0d8..1807fb7747 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -17,7 +17,7 @@ from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP, maybe_update_radar_points, plot_lead, plot_model, pygame_modules_have_loaded) -from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcClient, VisionStreamType os.environ['BASEDIR'] = BASEDIR @@ -99,7 +99,7 @@ def ui_thread(addr): draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles) - vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_ROAD, True) + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) while 1: list(pygame.event.get()) @@ -111,21 +111,17 @@ def ui_thread(addr): if not vipc_client.is_connected(): vipc_client.connect(True) - rgb_img_raw = vipc_client.recv() + yuv_img_raw = vipc_client.recv() - if rgb_img_raw is None or not rgb_img_raw.any(): + if yuv_img_raw is None or not yuv_img_raw.any(): continue - num_px = len(rgb_img_raw) // 3 - imgff_shape = (vipc_client.height, vipc_client.width, 3) + imgff = np.frombuffer(yuv_img_raw, dtype=np.uint8).reshape((vipc_client.height * 3 // 2, vipc_client.width)) + num_px = vipc_client.width * vipc_client.height + bgr = cv2.cvtColor(imgff, cv2.COLOR_YUV2RGB_I420) - if imgff is None or imgff.shape != imgff_shape: - imgff = np.zeros(imgff_shape, dtype=np.uint8) - - imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((vipc_client.height, vipc_client.width, 3)) - imgff = imgff[:, :, ::-1] # Convert BGR to RGB zoom_matrix = _BB_TO_FULL_FRAME[num_px] - cv2.warpAffine(imgff, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) + cv2.warpAffine(bgr, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) intrinsic_matrix = _INTRINSICS[num_px] diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 5d40c8ce94..dbf8f578fd 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -15,7 +15,7 @@ import pyopencl.array as cl_array import cereal.messaging as messaging from cereal import log -from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from cereal.visionipc import VisionIpcServer, VisionStreamType from common.basedir import BASEDIR from common.numpy_fast import clip from common.params import Params