diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f4fa0d8140f491..5a4b7ef4dda41a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -25,17 +25,12 @@ from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.driver_monitor import DriverStatus +from selfdrive.locationd.calibration_values import Calibration, Filter ThermalStatus = log.ThermalData.ThermalStatus State = log.Live100Data.ControlState -class Calibration: - UNCALIBRATED = 0 - CALIBRATED = 1 - INVALID = 2 - - def isActive(state): """Check if the actuators are enabled""" return state in [State.enabled, State.softDisabling] @@ -293,7 +288,10 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, extra_text_1, extra_text_2 = "", "" if e == "calibrationIncomplete": extra_text_1 = str(cal_perc) + "%" - extra_text_2 = "35 kph" if is_metric else "15 mph" + if is_metric: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" + else: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(sec_since_boot()) diff --git a/selfdrive/locationd/calibration_values.py b/selfdrive/locationd/calibration_values.py new file mode 100644 index 00000000000000..f2909007594924 --- /dev/null +++ b/selfdrive/locationd/calibration_values.py @@ -0,0 +1,11 @@ +import math + +class Filter: + MIN_SPEED = 7 # m/s (~15.5mph) + MAX_YAW_RATE = math.radians(3) # per second + +class Calibration: + UNCALIBRATED = 0 + CALIBRATED = 1 + INVALID = 2 + diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 695e698cee970c..36d76aa59e7600 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -3,10 +3,10 @@ import zmq import cv2 import copy -import math import json import numpy as np import selfdrive.messaging as messaging +from selfdrive.locationd.calibration_values import Calibration, Filter from selfdrive.swaglog import cloudlog from selfdrive.services import service_list from common.params import Params @@ -17,8 +17,6 @@ eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W -MIN_SPEED_FILTER = 7 # m/s (~15.5mph) -MAX_YAW_RATE_FILTER = math.radians(3) # per second FRAMES_NEEDED = 120 # allow to update VP every so many frames VP_CYCLES_NEEDED = 2 CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED @@ -42,12 +40,6 @@ ffi, lib = ffi_wrap('get_vp', c_code, c_header) -class Calibration: - UNCALIBRATED = 0 - CALIBRATED = 1 - INVALID = 2 - - def increment_grid_c(grid, lines, n): lib.increment_grid(ffi.cast("double *", grid.ctypes.data), ffi.cast("double *", lines.ctypes.data), @@ -131,8 +123,8 @@ def rescale_grid(self): def update(self, uvs, yaw_rate, speed): if len(uvs) < 10 or \ - abs(yaw_rate) > MAX_YAW_RATE_FILTER or \ - speed < MIN_SPEED_FILTER: + abs(yaw_rate) > Filter.MAX_YAW_RATE or \ + speed < Filter.MIN_SPEED: return rot_speeds = np.array([0.,0.,-yaw_rate]) uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))