liveLocationKalman, livePose, comfortBrake2.4, korean message (#159)
* fix routeinfo... * livePose -> liveLocationKalman * fix.. * for debug.. socketmaster * fix.. SCC track * fix.. carrotman * comfortBrake 2.5 -> 2.4 * fix.. * test gps debug * test gps * revert * test gps msg * test restart * ff * ff * ff * liveDelay(SAD), latSmoothSeconds (#158) * SAD default: 0 (use liveDelay)
This commit is contained in:
carrot
GitHub
parent
8965cb4d5c
commit
f97149e1b5
@ -186,6 +186,9 @@ SubMaster::~SubMaster() {
|
||||
|
||||
PubMaster::PubMaster(const std::vector<const char *> &service_list) {
|
||||
for (auto name : service_list) {
|
||||
if (services.count(name) == 0) {
|
||||
printf("PubMaster Error: unknown service '%s'\n", name);
|
||||
}
|
||||
assert(services.count(name) > 0);
|
||||
PubSocket *socket = PubSocket::create(message_context.context(), name);
|
||||
assert(socket);
|
||||
|
||||
@ -36,7 +36,7 @@ _services: dict[str, tuple] = {
|
||||
"errorLogMessage": (True, 0., 1),
|
||||
"liveCalibration": (True, 4., 4),
|
||||
"liveTorqueParameters": (True, 4., 1),
|
||||
#"liveDelay": (True, 4., 1),
|
||||
"liveDelay": (True, 4., 1),
|
||||
"androidLog": (True, 0.),
|
||||
"carState": (True, 100., 10),
|
||||
"carControl": (True, 100., 10),
|
||||
@ -51,7 +51,8 @@ _services: dict[str, tuple] = {
|
||||
"gnssMeasurements": (True, 10., 10),
|
||||
"clocks": (True, 0.1, 1),
|
||||
"ubloxRaw": (True, 20.),
|
||||
"livePose": (True, 20., 4),
|
||||
#"livePose": (True, 20., 4),
|
||||
"liveLocationKalman": (True, 20., 5),
|
||||
"liveParameters": (True, 20., 5),
|
||||
"cameraOdometry": (True, 20., 10),
|
||||
"thumbnail": (True, 1 / 60., 1),
|
||||
|
||||
@ -8,12 +8,12 @@ import functools
|
||||
import threading
|
||||
from cereal.messaging import PubMaster
|
||||
from cereal.services import SERVICE_LIST
|
||||
from openpilot.common.mock.generators import generate_livePose
|
||||
from openpilot.common.mock.generators import generate_liveLocationKalman
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
|
||||
|
||||
MOCK_GENERATOR = {
|
||||
"livePose": generate_livePose
|
||||
"liveLocationKalman": generate_liveLocationKalman
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,14 +1,20 @@
|
||||
from cereal import messaging
|
||||
|
||||
|
||||
def generate_livePose():
|
||||
msg = messaging.new_message('livePose')
|
||||
meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
|
||||
msg.livePose.orientationNED = meas
|
||||
msg.livePose.velocityDevice = meas
|
||||
msg.livePose.angularVelocityDevice = meas
|
||||
msg.livePose.accelerationDevice = meas
|
||||
msg.livePose.inputsOK = True
|
||||
msg.livePose.posenetOK = True
|
||||
msg.livePose.sensorsOK = True
|
||||
LOCATION1 = (32.7174, -117.16277)
|
||||
LOCATION2 = (32.7558, -117.2037)
|
||||
|
||||
LLK_DECIMATION = 10
|
||||
RENDER_FRAMES = 15
|
||||
DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
|
||||
|
||||
|
||||
def generate_liveLocationKalman(location=LOCATION1):
|
||||
msg = messaging.new_message('liveLocationKalman')
|
||||
msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
|
||||
msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||||
msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||||
msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
|
||||
msg.liveLocationKalman.status = 'valid'
|
||||
msg.liveLocationKalman.gpsOK = True
|
||||
return msg
|
||||
|
||||
@ -195,6 +195,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"MyDrivingModeAuto", PERSISTENT},
|
||||
{"TrafficLightDetectMode", PERSISTENT},
|
||||
{"SteerActuatorDelay", PERSISTENT},
|
||||
{"SteerSmoothSec", PERSISTENT},
|
||||
{"CruiseOnDist", PERSISTENT},
|
||||
{"CruiseMaxVals1", PERSISTENT},
|
||||
{"CruiseMaxVals2", PERSISTENT},
|
||||
|
||||
@ -90,7 +90,8 @@ function launch {
|
||||
fi
|
||||
|
||||
# events language init
|
||||
LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
|
||||
#LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
|
||||
LANG=$(cat /data/params/d/LanguageSetting)
|
||||
EVENTSTAT=$(git status)
|
||||
|
||||
# events.py 한글로 변경 및 파일이 교체된 상태인지 확인
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
git pull
|
||||
tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; tmux new -s comma -d "/data/openpilot/launch_openpilot.sh"
|
||||
tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; sleep 1;tmux new -s comma -d "/data/openpilot/launch_openpilot.sh"
|
||||
|
||||
@ -162,6 +162,9 @@ class Car:
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
|
||||
#self.t1 = time.monotonic()
|
||||
#self.t2 = self.t1
|
||||
#self.t3 = self.t2
|
||||
# card is driven by can recv, expected at 100Hz
|
||||
self.rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
|
||||
@ -182,6 +185,7 @@ class Car:
|
||||
#RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, can_list)
|
||||
|
||||
self.sm.update(0)
|
||||
#self.t1 = time.monotonic()
|
||||
|
||||
can_rcv_valid = len(can_strs) > 0
|
||||
|
||||
@ -193,9 +197,11 @@ class Car:
|
||||
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
|
||||
|
||||
RD: structs.RadarDataT | None = self.RI.update_carrot(CS.vEgo, rcv_time, can_list)
|
||||
#self.t2 = time.monotonic()
|
||||
|
||||
#self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.sm, self.is_metric)
|
||||
#self.t3 = time.monotonic()
|
||||
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
|
||||
# Use CarState w/ buttons from the step selfdrived enables on
|
||||
self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
|
||||
@ -285,13 +291,15 @@ class Car:
|
||||
try:
|
||||
t.start()
|
||||
while True:
|
||||
#start = time.monotonic()
|
||||
self.step()
|
||||
#if self.sm.frame % 100 == 0:
|
||||
# print(f"elapsed time = {(self.t1 - start)*1000.:.2f}, {(self.t2 - self.t1)*1000.:.2f}, {(self.t3 - self.t1)*1000.:.2f}, {(time.monotonic() - self.t1)*1000.:.2f}")
|
||||
self.rk.monitor_time()
|
||||
finally:
|
||||
e.set()
|
||||
t.join()
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process(4, Priority.CTRL_HIGH)
|
||||
car = Car()
|
||||
|
||||
@ -80,7 +80,7 @@ class CarrotPlanner:
|
||||
|
||||
self.stop_distance = 6.0
|
||||
self.trafficStopDistanceAdjust = 1.0 #params.get_float("TrafficStopDistanceAdjust") / 100.
|
||||
self.comfortBrake = 2.5
|
||||
self.comfortBrake = 2.4
|
||||
self.comfort_brake = self.comfortBrake
|
||||
|
||||
self.soft_hold_active = 0
|
||||
@ -121,7 +121,6 @@ class CarrotPlanner:
|
||||
self.jerk_factor_apply = 1.0
|
||||
|
||||
self.j_lead_factor = 0.0
|
||||
self.carrot_mpc1 = 0.0
|
||||
|
||||
self.activeCarrot = 0
|
||||
self.xDistToTurn = 0
|
||||
|
||||
@ -238,7 +238,7 @@ class CarrotMan:
|
||||
print("************************************************CarrotMan init************************************************")
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'navInstruction'])
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman', 'navInstruction'])
|
||||
self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
|
||||
|
||||
self.carrot_serv = CarrotServ()
|
||||
@ -756,6 +756,11 @@ class CarrotMan:
|
||||
print("Received points from navd:", len(self.navi_points))
|
||||
self.navd_active = True
|
||||
|
||||
# 경로수신 -> carrotman active되고 약간의 시간지연이 발생함..
|
||||
self.carrot_serv.active_count = 80
|
||||
self.carrot_serv.active_sdi_count = self.carrot_serv.active_sdi_count_max
|
||||
self.carrot_serv.active_carrot = 2
|
||||
|
||||
coords = [{"latitude": c.latitude, "longitude": c.longitude} for c in coords]
|
||||
#print("navdNaviPoints=", self.navi_points)
|
||||
else:
|
||||
@ -1265,11 +1270,14 @@ class CarrotServ:
|
||||
self.xSpdDist = 0
|
||||
|
||||
def _update_gps(self, v_ego, sm):
|
||||
if not sm.updated['carState'] or not sm.updated['carControl']:
|
||||
llk = 'liveLocationKalman'
|
||||
location = sm[llk]
|
||||
#print(f"location = {sm.valid[llk]}, {sm.updated[llk]}, {sm.recv_frame[llk]}, {sm.recv_time[llk]}")
|
||||
if not sm.updated['carState'] or not sm.updated['carControl']: # or not sm.updated[llk]:
|
||||
return self.nPosAngle
|
||||
CS = sm['carState']
|
||||
CC = sm['carControl']
|
||||
self.gps_valid = False #(location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
self.gps_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
|
||||
now = time.monotonic()
|
||||
gps_updated_phone = (now - self.last_update_gps_time_phone) < 3
|
||||
@ -1278,8 +1286,8 @@ class CarrotServ:
|
||||
bearing = self.nPosAngle
|
||||
if gps_updated_phone:
|
||||
self.bearing_offset = 0.0
|
||||
elif len(CC.orientationNED) == 3:
|
||||
bearing = math.degrees(CC.orientationNED[2])
|
||||
elif sm.valid[llk]:
|
||||
bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
if self.gps_valid:
|
||||
self.bearing_offset = 0.0
|
||||
elif self.active_carrot > 0:
|
||||
@ -1293,7 +1301,7 @@ class CarrotServ:
|
||||
|
||||
external_gps_update_timedout = not (gps_updated_phone or gps_updated_navi)
|
||||
#print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}")
|
||||
if False: #self.gps_valid and external_gps_update_timedout: # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
|
||||
if self.gps_valid and external_gps_update_timedout: # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
|
||||
self.vpPosPointLatNavi = location.positionGeodetic.value[0]
|
||||
self.vpPosPointLonNavi = location.positionGeodetic.value[1]
|
||||
self.last_calculate_gps_time = now #sm.recv_time[llk]
|
||||
|
||||
@ -213,15 +213,28 @@
|
||||
"group": "조향튜닝",
|
||||
"name": "SteerActuatorDelay",
|
||||
"title": "SteerActuatorDelay(30)",
|
||||
"descr": "조향지연값",
|
||||
"descr": "조향지연값, 0:LiveDelay",
|
||||
"egroup": "LAT",
|
||||
"etitle": "SteerActuatorDelay(30)",
|
||||
"edescr": "",
|
||||
"min": 1,
|
||||
"edescr": "0:LiveDelay",
|
||||
"min": 0,
|
||||
"max": 300,
|
||||
"default": 30,
|
||||
"unit": 1
|
||||
},
|
||||
{
|
||||
"group": "조향튜닝",
|
||||
"name": "SteerSmoothSec",
|
||||
"title": "조향필터링타임x0.01(13)",
|
||||
"descr": "값이커지면 필터링을 더 많이 함.",
|
||||
"egroup": "LAT",
|
||||
"etitle": "SteerSmoothSecx0.01(13)",
|
||||
"edescr": "",
|
||||
"min": 1,
|
||||
"max": 100,
|
||||
"default": 13,
|
||||
"unit": 1
|
||||
},
|
||||
{
|
||||
"group": "크루즈",
|
||||
"name": "CruiseOnDist",
|
||||
|
||||
@ -20,13 +20,11 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
|
||||
|
||||
State = log.SelfdriveState.OpenpilotState
|
||||
LaneChangeState = log.LaneChangeState
|
||||
@ -47,8 +45,8 @@ class Controls:
|
||||
self.disable_dm = False
|
||||
|
||||
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'carrotMan', 'lateralPlan', 'radarState',
|
||||
'liveCalibration', 'liveLocationKalman', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'liveDelay', 'carrotMan', 'lateralPlan', 'radarState',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
@ -57,9 +55,6 @@ class Controls:
|
||||
self.desired_curvature = 0.0
|
||||
self.yStd = 0.0
|
||||
|
||||
self.pose_calibrator = PoseCalibrator()
|
||||
self.calibrated_pose: Pose | None = None
|
||||
|
||||
self.LoC = LongControl(self.CP)
|
||||
self.VM = VehicleModel(self.CP)
|
||||
self.LaC: LatControl
|
||||
@ -72,11 +67,6 @@ class Controls:
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
if self.sm.updated["liveCalibration"]:
|
||||
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
|
||||
if self.sm.updated["livePose"]:
|
||||
device_pose = Pose.from_live_pose(self.sm['livePose'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
|
||||
|
||||
def state_control(self):
|
||||
CS = self.sm['carState']
|
||||
@ -143,11 +133,13 @@ class Controls:
|
||||
curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed)
|
||||
self.lanefull_mode_enabled = (lat_plan.useLaneLines and self.params.get_int("UseLaneLineSpeedApply") > 0 and
|
||||
curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
|
||||
|
||||
steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS
|
||||
lat_smooth_seconds = self.params.get_float("SteerSmoothSec") * 0.01
|
||||
steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
|
||||
if steer_actuator_delay == 0.0:
|
||||
steer_actuator_delay = self.sm['liveDelay'].lateralDelay
|
||||
|
||||
if len(model_v2.position.yStd) > 0:
|
||||
yStd = np.interp(steer_actuator_delay + LAT_SMOOTH_SECONDS, ModelConstants.T_IDXS, model_v2.position.yStd)
|
||||
yStd = np.interp(steer_actuator_delay + lat_smooth_seconds, ModelConstants.T_IDXS, model_v2.position.yStd)
|
||||
self.yStd = yStd * 0.1 + self.yStd * 0.9
|
||||
else:
|
||||
self.yStd = 0.0
|
||||
@ -163,8 +155,8 @@ class Controls:
|
||||
return alpha * val + (1 - alpha) * prev_val
|
||||
|
||||
t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL
|
||||
curvature = np.interp(steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)
|
||||
new_desired_curvature = smooth_value(curvature, self.desired_curvature, LAT_SMOOTH_SECONDS)
|
||||
curvature = np.interp(steer_actuator_delay + lat_smooth_seconds + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)
|
||||
new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature
|
||||
|
||||
@ -173,7 +165,7 @@ class Controls:
|
||||
actuators.curvature = float(self.desired_curvature)
|
||||
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited_by_controls, self.desired_curvature,
|
||||
self.calibrated_pose, curvature_limited) # TODO what if not available
|
||||
self.sm['liveLocationKalman'], curvature_limited) # TODO what if not available
|
||||
actuators.torque = float(steer)
|
||||
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
||||
actuators.yStd = float(self.yStd)
|
||||
@ -194,9 +186,12 @@ class Controls:
|
||||
|
||||
# Orientation and angle rates can be useful for carcontroller
|
||||
# Only calibrated (car) frame is relevant for the carcontroller
|
||||
if self.calibrated_pose is not None:
|
||||
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
||||
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
||||
orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
|
||||
if len(orientation_value) > 2:
|
||||
CC.orientationNED = orientation_value
|
||||
angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
|
||||
if len(angular_rate_value) > 2:
|
||||
CC.angularVelocity = angular_rate_value
|
||||
|
||||
CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
|
||||
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
|
||||
|
||||
@ -17,7 +17,7 @@ class LatControl(ABC):
|
||||
self.steer_max = 1.0
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
|
||||
@ -13,7 +13,7 @@ class LatControlAngle(LatControl):
|
||||
self.sat_check_min_speed = 5.
|
||||
self.angle_steers_des = 0.0
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if not active:
|
||||
|
||||
@ -17,7 +17,7 @@ class LatControlPID(LatControl):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
||||
@ -57,7 +57,7 @@ class LatControlTorque(LatControl):
|
||||
self.torque_params.latAccelOffset = latAccelOffset
|
||||
self.torque_params.friction = friction
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited):
|
||||
self.frame += 1
|
||||
if self.frame % 10 == 0:
|
||||
lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
|
||||
@ -94,9 +94,8 @@ class LatControlTorque(LatControl):
|
||||
actual_curvature = actual_curvature_vm
|
||||
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
|
||||
else:
|
||||
assert calibrated_pose is not None
|
||||
actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
|
||||
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
|
||||
actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
|
||||
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
|
||||
curvature_deadzone = 0.0
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
|
||||
|
||||
@ -453,8 +453,10 @@ class RadarD:
|
||||
|
||||
v_ego = self.v_ego
|
||||
ready = self.ready
|
||||
## SCC레이더는 일단 보관하고 리스트에서 삭제...
|
||||
## SCC레이더는 일단 보관하고 리스트에서 삭제... (SCC Track은 0,1번으로 들어옴)
|
||||
track_scc = tracks.get(0)
|
||||
if track_scc is None:
|
||||
track_scc = tracks.get(1)
|
||||
#if track_scc is not None:
|
||||
# del tracks[0] ## tracks에서 삭제하면안됨... ㅠㅠ
|
||||
|
||||
|
||||
@ -1,15 +1,17 @@
|
||||
Import('env', 'rednose')
|
||||
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
|
||||
|
||||
loc_libs = [messaging, common, 'pthread', 'dl']
|
||||
|
||||
# build ekf models
|
||||
rednose_gen_dir = 'models/generated'
|
||||
rednose_gen_deps = [
|
||||
"models/constants.py",
|
||||
]
|
||||
pose_ekf = env.RednoseCompileFilter(
|
||||
target='pose',
|
||||
filter_gen_script='models/pose_kf.py',
|
||||
live_ekf = env.RednoseCompileFilter(
|
||||
target='live',
|
||||
filter_gen_script='models/live_kf.py',
|
||||
output_dir=rednose_gen_dir,
|
||||
extra_gen_artifacts=[],
|
||||
extra_gen_artifacts=['live_kf_constants.h'],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
car_ekf = env.RednoseCompileFilter(
|
||||
@ -19,3 +21,17 @@ car_ekf = env.RednoseCompileFilter(
|
||||
extra_gen_artifacts=[],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
|
||||
# locationd build
|
||||
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
|
||||
|
||||
lenv = env.Clone()
|
||||
# ekf filter libraries need to be linked, even if no symbols are used
|
||||
if arch != "Darwin":
|
||||
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
|
||||
|
||||
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
|
||||
lenv.Depends(locationd, rednose)
|
||||
lenv.Depends(locationd, live_ekf)
|
||||
|
||||
@ -11,7 +11,7 @@ from cereal.services import SERVICE_LIST
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
|
||||
from openpilot.selfdrive.locationd.helpers import fft_next_good_size, parabolic_peak_interp
|
||||
|
||||
BLOCK_SIZE = 100
|
||||
BLOCK_NUM = 50
|
||||
@ -106,6 +106,8 @@ class Points:
|
||||
self.okay.append(okay)
|
||||
self.desired.append(desired)
|
||||
self.actual.append(actual)
|
||||
#if okay:
|
||||
# print(f"lagd: {t:.3f} {desired:.4f} {actual:.4f} {self.num_okay}/{self.num_points}")
|
||||
|
||||
def get(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
|
||||
return np.array(self.times), np.array(self.desired), np.array(self.actual), np.array(self.okay)
|
||||
@ -148,7 +150,7 @@ class BlockAverage:
|
||||
|
||||
|
||||
class LateralLagEstimator:
|
||||
inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"}
|
||||
inputs = {"carControl", "carState", "controlsState", "liveLocationKalman"}
|
||||
|
||||
def __init__(self, CP: car.CarParams, dt: float,
|
||||
block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
|
||||
@ -185,7 +187,7 @@ class LateralLagEstimator:
|
||||
self.last_pose_invalid_t = 0.0
|
||||
self.last_estimate_t = 0.0
|
||||
|
||||
self.calibrator = PoseCalibrator()
|
||||
self.calib_valid = False
|
||||
|
||||
self.reset(self.initial_lag, 0)
|
||||
|
||||
@ -237,14 +239,11 @@ class LateralLagEstimator:
|
||||
elif which == "controlsState":
|
||||
self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
|
||||
self.desired_curvature = msg.desiredCurvature
|
||||
elif which == "liveCalibration":
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
elif which == "livePose":
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
self.yaw_rate = calibrated_pose.angular_velocity.yaw
|
||||
self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
|
||||
self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
|
||||
elif which == 'liveLocationKalman':
|
||||
self.yaw_rate = msg.angularVelocityCalibrated.value[2]
|
||||
self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
||||
self.pose_valid = msg.angularVelocityCalibrated.valid and msg.posenetOK and msg.inputsOK
|
||||
self.calib_valid = msg.status == log.LiveLocationKalman.Status.valid
|
||||
self.t = t
|
||||
|
||||
def points_enough(self):
|
||||
@ -261,7 +260,7 @@ class LateralLagEstimator:
|
||||
turning = np.abs(self.yaw_rate) >= self.min_yr
|
||||
sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK
|
||||
la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff
|
||||
calib_valid = self.calibrator.calib_valid
|
||||
calib_valid = self.calib_valid #self.calibrator.calib_valid
|
||||
|
||||
if not self.lat_active:
|
||||
self.last_lat_inactive_t = self.t
|
||||
@ -343,12 +342,12 @@ def main():
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveDelay'])
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose')
|
||||
sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'controlsState', 'carControl'], poll='liveLocationKalman')
|
||||
|
||||
params_reader = Params()
|
||||
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
|
||||
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['liveLocationKalman'].frequency)
|
||||
if (initial_lag_params := retrieve_initial_lag(params_reader, CP)) is not None:
|
||||
lag, valid_blocks = initial_lag_params
|
||||
lag_learner.reset(lag, valid_blocks)
|
||||
|
||||
@ -160,18 +160,19 @@ class CarKalman(KalmanFilter):
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
|
||||
|
||||
def __init__(self, generated_dir):
|
||||
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
|
||||
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
|
||||
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
|
||||
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
|
||||
dim_state = self.initial_x.shape[0]
|
||||
dim_state_err = self.P_initial.shape[0]
|
||||
x_init = self.initial_x
|
||||
x_init[States.STEER_RATIO] = steer_ratio
|
||||
x_init[States.STIFFNESS] = stiffness_factor
|
||||
x_init[States.ANGLE_OFFSET] = angle_offset
|
||||
|
||||
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
|
||||
self.filter.set_global("mass", mass)
|
||||
self.filter.set_global("rotational_inertia", rotational_inertia)
|
||||
self.filter.set_global("center_to_front", center_to_front)
|
||||
self.filter.set_global("center_to_rear", center_to_rear)
|
||||
self.filter.set_global("stiffness_front", stiffness_front)
|
||||
self.filter.set_global("stiffness_rear", stiffness_rear)
|
||||
if P_initial is not None:
|
||||
self.P_initial = P_initial
|
||||
# init filter
|
||||
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
|
||||
dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@ -1,8 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import math
|
||||
import json
|
||||
import numpy as np
|
||||
import capnp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car, log
|
||||
@ -10,14 +10,14 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
|
||||
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
|
||||
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
|
||||
ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
|
||||
ROLL_LOWERED_MAX = np.radians(8)
|
||||
ROLL_STD_MAX = np.radians(1.5)
|
||||
ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
|
||||
ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
|
||||
ROLL_LOWERED_MAX = math.radians(8)
|
||||
ROLL_STD_MAX = math.radians(1.5)
|
||||
LATERAL_ACC_SENSOR_THRESHOLD = 4.0
|
||||
OFFSET_MAX = 10.0
|
||||
OFFSET_LOWERED_MAX = 8.0
|
||||
@ -25,61 +25,32 @@ MIN_ACTIVE_SPEED = 1.0
|
||||
LOW_ACTIVE_SPEED = 10.0
|
||||
|
||||
|
||||
class VehicleParamsLearner:
|
||||
def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
|
||||
self.kf = CarKalman(GENERATED_DIR)
|
||||
class ParamsLearner:
|
||||
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
|
||||
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
|
||||
|
||||
self.x_initial = CarKalman.initial_x.copy()
|
||||
self.x_initial[States.STEER_RATIO] = steer_ratio
|
||||
self.x_initial[States.STIFFNESS] = stiffness_factor
|
||||
self.x_initial[States.ANGLE_OFFSET] = angle_offset
|
||||
self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
|
||||
self.kf.filter.set_global("mass", CP.mass)
|
||||
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
|
||||
self.kf.filter.set_global("center_to_front", CP.centerToFront)
|
||||
self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
|
||||
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
|
||||
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
|
||||
|
||||
self.kf.set_globals(
|
||||
mass=CP.mass,
|
||||
rotational_inertia=CP.rotationalInertia,
|
||||
center_to_front=CP.centerToFront,
|
||||
center_to_rear=CP.wheelbase - CP.centerToFront,
|
||||
stiffness_front=CP.tireStiffnessFront,
|
||||
stiffness_rear=CP.tireStiffnessRear
|
||||
)
|
||||
self.active = False
|
||||
|
||||
self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
|
||||
self.speed = 0.0
|
||||
self.yaw_rate = 0.0
|
||||
self.yaw_rate_std = 0.0
|
||||
self.roll = 0.0
|
||||
self.steering_angle = 0.0
|
||||
|
||||
self.calibrator = PoseCalibrator()
|
||||
def handle_log(self, t, which, msg):
|
||||
if which == 'liveLocationKalman':
|
||||
self.yaw_rate = msg.angularVelocityCalibrated.value[2]
|
||||
self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
||||
|
||||
self.observed_speed = 0.0
|
||||
self.observed_yaw_rate = 0.0
|
||||
self.observed_roll = 0.0
|
||||
|
||||
self.avg_offset_valid = True
|
||||
self.total_offset_valid = True
|
||||
self.roll_valid = True
|
||||
|
||||
self.reset(None)
|
||||
|
||||
def reset(self, t: float | None):
|
||||
self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
|
||||
|
||||
self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
|
||||
self.avg_angle_offset = self.angle_offset
|
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
|
||||
if which == 'livePose':
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
|
||||
yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
|
||||
yaw_rate_valid = msg.angularVelocityDevice.valid
|
||||
yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s
|
||||
yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
|
||||
if not yaw_rate_valid:
|
||||
# This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
|
||||
yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
|
||||
self.observed_yaw_rate = yaw_rate
|
||||
|
||||
localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
|
||||
localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
|
||||
localizer_roll = msg.orientationNED.value[0]
|
||||
localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
|
||||
roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
|
||||
if roll_valid:
|
||||
roll = localizer_roll
|
||||
@ -89,18 +60,18 @@ class VehicleParamsLearner:
|
||||
# This is done to bound the road roll estimate when localizer values are invalid
|
||||
roll = 0.0
|
||||
roll_std = np.radians(10.0)
|
||||
self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
|
||||
self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
|
||||
|
||||
if self.active:
|
||||
if msg.posenetOK:
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_YAW_RATE,
|
||||
np.array([[-self.observed_yaw_rate]]),
|
||||
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
||||
np.array([[-self.yaw_rate]]),
|
||||
np.array([np.atleast_2d(self.yaw_rate_std**2)]))
|
||||
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_ROLL,
|
||||
np.array([[self.observed_roll]]),
|
||||
np.array([[self.roll]]),
|
||||
np.array([np.atleast_2d(roll_std**2)]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
|
||||
|
||||
@ -112,83 +83,21 @@ class VehicleParamsLearner:
|
||||
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
|
||||
|
||||
elif which == 'liveCalibration':
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
|
||||
elif which == 'carState':
|
||||
steering_angle = msg.steeringAngleDeg
|
||||
self.steering_angle = msg.steeringAngleDeg
|
||||
self.speed = msg.vEgo
|
||||
|
||||
in_linear_region = abs(steering_angle) < 45
|
||||
self.observed_speed = msg.vEgo
|
||||
self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
|
||||
in_linear_region = abs(self.steering_angle) < 45
|
||||
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
|
||||
|
||||
if self.active:
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
|
||||
|
||||
if not self.active:
|
||||
# Reset time when stopped so uncertainty doesn't grow
|
||||
self.kf.filter.set_filter_time(t) # type: ignore
|
||||
self.kf.filter.reset_rewind() # type: ignore
|
||||
|
||||
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
|
||||
x = self.kf.x
|
||||
P = np.sqrt(self.kf.P.diagonal())
|
||||
if not np.all(np.isfinite(x)):
|
||||
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
|
||||
self.reset(self.kf.t)
|
||||
x = self.kf.x
|
||||
|
||||
self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
|
||||
self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
|
||||
self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
|
||||
self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
|
||||
self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
|
||||
roll_std = float(P[States.ROAD_ROLL].item())
|
||||
if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
|
||||
# Account for the opposite signs of the yaw rates
|
||||
# At low speeds, bumping into a curb can cause the yaw rate to be very high
|
||||
sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
|
||||
else:
|
||||
sensors_valid = True
|
||||
self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
|
||||
self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
|
||||
self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
|
||||
|
||||
msg = messaging.new_message('liveParameters')
|
||||
|
||||
msg.valid = valid
|
||||
|
||||
liveParameters = msg.liveParameters
|
||||
liveParameters.posenetValid = True
|
||||
liveParameters.sensorValid = sensors_valid
|
||||
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
|
||||
liveParameters.roll = float(self.roll)
|
||||
liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
|
||||
liveParameters.angleOffsetDeg = float(self.angle_offset)
|
||||
liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
|
||||
liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
|
||||
liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
|
||||
liveParameters.angleOffsetValid = bool(self.total_offset_valid)
|
||||
liveParameters.valid = all((
|
||||
liveParameters.angleOffsetAverageValid,
|
||||
liveParameters.angleOffsetValid ,
|
||||
self.roll_valid,
|
||||
roll_std < ROLL_STD_MAX,
|
||||
liveParameters.stiffnessFactorValid,
|
||||
liveParameters.steerRatioValid,
|
||||
))
|
||||
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
|
||||
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
|
||||
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
|
||||
if debug:
|
||||
liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
|
||||
liveParameters.debugFilterState.value = x.tolist()
|
||||
liveParameters.debugFilterState.std = P.tolist()
|
||||
|
||||
return msg
|
||||
self.kf.filter.set_filter_time(t)
|
||||
self.kf.filter.reset_rewind()
|
||||
|
||||
|
||||
def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
|
||||
@ -199,67 +108,6 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
|
||||
return current_valid
|
||||
|
||||
|
||||
# TODO: Remove this function after few releases (added in 0.9.9)
|
||||
def migrate_cached_vehicle_params_if_needed(params: Params):
|
||||
last_parameters_data_old = params.get("LiveParameters")
|
||||
last_parameters_data = params.get("LiveParametersV2")
|
||||
if last_parameters_data_old is None or last_parameters_data is not None:
|
||||
return
|
||||
|
||||
try:
|
||||
last_parameters_dict = json.loads(last_parameters_data_old)
|
||||
last_parameters_msg = messaging.new_message('liveParameters')
|
||||
last_parameters_msg.liveParameters.valid = True
|
||||
last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio']
|
||||
last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor']
|
||||
last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg']
|
||||
params.put("LiveParametersV2", last_parameters_msg.to_bytes())
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Failed to perform parameter migration: {e}")
|
||||
params.remove("LiveParameters")
|
||||
|
||||
|
||||
def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
|
||||
last_parameters_data = params.get("LiveParametersV2")
|
||||
last_carparams_data = params.get("CarParamsPrevRoute")
|
||||
|
||||
steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
|
||||
|
||||
retrieve_success = False
|
||||
if last_parameters_data is not None and last_carparams_data is not None:
|
||||
try:
|
||||
with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
|
||||
lp = last_lp_msg.liveParameters
|
||||
# Check if car model matches
|
||||
if last_CP.carFingerprint != CP.carFingerprint:
|
||||
raise Exception("Car model mismatch")
|
||||
|
||||
# Check if starting values are sane
|
||||
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
|
||||
steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
|
||||
if not steer_ratio_sane:
|
||||
raise Exception(f"Invalid starting values found {lp}")
|
||||
|
||||
initial_filter_std = np.array(lp.debugFilterState.std)
|
||||
if debug and len(initial_filter_std) != 0:
|
||||
p_initial = np.diag(initial_filter_std)
|
||||
|
||||
steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
|
||||
retrieve_success = True
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Failed to retrieve initial values: {e}")
|
||||
|
||||
if not replay:
|
||||
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
|
||||
# Without a way to detect this we have to reset the stiffness every drive
|
||||
stiffness_factor = 1.0
|
||||
|
||||
if not retrieve_success:
|
||||
cloudlog.info("Parameter learner resetting to default values")
|
||||
|
||||
return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
@ -267,15 +115,64 @@ def main():
|
||||
REPLAY = bool(int(os.getenv("REPLAY", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveParameters'])
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
|
||||
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman')
|
||||
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
params_reader = Params()
|
||||
# wait for stats about the car to come in from controls
|
||||
cloudlog.info("paramsd is waiting for CarParams")
|
||||
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("paramsd got CarParams")
|
||||
|
||||
migrate_cached_vehicle_params_if_needed(params)
|
||||
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
|
||||
|
||||
steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
|
||||
learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
|
||||
params = params_reader.get("LiveParameters")
|
||||
|
||||
# Check if car model matches
|
||||
if params is not None:
|
||||
params = json.loads(params)
|
||||
if params.get('carFingerprint', None) != CP.carFingerprint:
|
||||
cloudlog.info("Parameter learner found parameters for wrong car.")
|
||||
params = None
|
||||
|
||||
# Check if starting values are sane
|
||||
if params is not None:
|
||||
try:
|
||||
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
|
||||
if not steer_ratio_sane:
|
||||
cloudlog.info(f"Invalid starting values found {params}")
|
||||
params = None
|
||||
except Exception as e:
|
||||
cloudlog.info(f"Error reading params {params}: {str(e)}")
|
||||
params = None
|
||||
|
||||
# TODO: cache the params with the capnp struct
|
||||
if params is None:
|
||||
params = {
|
||||
'carFingerprint': CP.carFingerprint,
|
||||
'steerRatio': CP.steerRatio,
|
||||
'stiffnessFactor': 1.0,
|
||||
'angleOffsetAverageDeg': 0.0,
|
||||
}
|
||||
cloudlog.info("Parameter learner resetting to default values")
|
||||
|
||||
if not REPLAY:
|
||||
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
|
||||
# Without a way to detect this we have to reset the stiffness every drive
|
||||
params['stiffnessFactor'] = 1.0
|
||||
|
||||
pInitial = None
|
||||
if DEBUG:
|
||||
pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None
|
||||
|
||||
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial)
|
||||
angle_offset_average = params['angleOffsetAverageDeg']
|
||||
angle_offset = angle_offset_average
|
||||
roll = 0.0
|
||||
avg_offset_valid = True
|
||||
total_offset_valid = True
|
||||
roll_valid = True
|
||||
params_memory = Params("/dev/shm/params")
|
||||
params_memory.remove("LastGPSPosition")
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
@ -285,14 +182,80 @@ def main():
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
learner.handle_log(t, which, sm[which])
|
||||
|
||||
if sm.updated['livePose']:
|
||||
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
|
||||
if sm.updated['liveLocationKalman']:
|
||||
location = sm['liveLocationKalman']
|
||||
if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
|
||||
bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
lat = location.positionGeodetic.value[0]
|
||||
lon = location.positionGeodetic.value[1]
|
||||
params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
|
||||
|
||||
x = learner.kf.x
|
||||
P = np.sqrt(learner.kf.P.diagonal())
|
||||
if not all(map(math.isfinite, x)):
|
||||
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
|
||||
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
|
||||
x = learner.kf.x
|
||||
|
||||
angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
|
||||
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
|
||||
angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
|
||||
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
|
||||
roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
|
||||
roll_std = float(P[States.ROAD_ROLL].item())
|
||||
if learner.active and learner.speed > LOW_ACTIVE_SPEED:
|
||||
# Account for the opposite signs of the yaw rates
|
||||
# At low speeds, bumping into a curb can cause the yaw rate to be very high
|
||||
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
|
||||
else:
|
||||
sensors_valid = True
|
||||
avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
|
||||
total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
|
||||
roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
|
||||
|
||||
msg = messaging.new_message('liveParameters')
|
||||
|
||||
liveParameters = msg.liveParameters
|
||||
liveParameters.posenetValid = True
|
||||
liveParameters.sensorValid = sensors_valid
|
||||
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
|
||||
liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr
|
||||
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
|
||||
liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
|
||||
liveParameters.roll = float(roll)
|
||||
liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
|
||||
liveParameters.angleOffsetAverageValid = bool(avg_offset_valid)
|
||||
liveParameters.angleOffsetDeg = float(angle_offset)
|
||||
liveParameters.angleOffsetValid = bool(total_offset_valid)
|
||||
liveParameters.valid = all((
|
||||
liveParameters.angleOffsetAverageValid,
|
||||
liveParameters.angleOffsetValid ,
|
||||
roll_valid,
|
||||
roll_std < ROLL_STD_MAX,
|
||||
liveParameters.stiffnessFactorValid,
|
||||
liveParameters.steerRatioValid,
|
||||
))
|
||||
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
|
||||
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
|
||||
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
|
||||
if DEBUG:
|
||||
liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
|
||||
liveParameters.debugFilterState.value = x.tolist()
|
||||
liveParameters.debugFilterState.std = P.tolist()
|
||||
|
||||
msg.valid = sm.all_checks()
|
||||
|
||||
msg_dat = msg.to_bytes()
|
||||
if sm.frame % 1200 == 0: # once a minute
|
||||
params.put_nonblocking("LiveParametersV2", msg_dat)
|
||||
params = {
|
||||
'carFingerprint': CP.carFingerprint,
|
||||
'steerRatio': liveParameters.steerRatio,
|
||||
'stiffnessFactor': liveParameters.stiffnessFactor,
|
||||
'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
|
||||
}
|
||||
params_reader.put_nonblocking("LiveParameters", json.dumps(params))
|
||||
|
||||
pm.send('liveParameters', msg_dat)
|
||||
pm.send('liveParameters', msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@ -9,7 +9,7 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
|
||||
from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
|
||||
|
||||
HISTORY = 5 # secs
|
||||
POINTS_PER_BUCKET = 1500
|
||||
@ -77,8 +77,6 @@ class TorqueEstimator(ParameterEstimator):
|
||||
self.offline_friction = CP.lateralTuning.torque.friction
|
||||
self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
|
||||
|
||||
self.calibrator = PoseCalibrator()
|
||||
|
||||
self.reset()
|
||||
|
||||
initial_params = {
|
||||
@ -173,18 +171,10 @@ class TorqueEstimator(ParameterEstimator):
|
||||
# TODO: check if high aEgo affects resulting lateral accel
|
||||
self.raw_points["vego"].append(msg.vEgo)
|
||||
self.raw_points["steer_override"].append(msg.steeringPressed)
|
||||
elif which == "liveCalibration":
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
|
||||
# calculate lateral accel from past steering torque
|
||||
elif which == "livePose":
|
||||
elif which == "liveLocationKalman":
|
||||
if len(self.raw_points['steer_torque']) == self.hist_len:
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
angular_velocity_calibrated = calibrated_pose.angular_velocity
|
||||
|
||||
yaw_rate = angular_velocity_calibrated.yaw
|
||||
roll = device_pose.orientation.roll
|
||||
yaw_rate = msg.angularVelocityCalibrated.value[2]
|
||||
roll = msg.orientationNED.value[0]
|
||||
# check lat active up to now (without lag compensation)
|
||||
lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
|
||||
self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
|
||||
@ -241,7 +231,7 @@ def main(demo=False):
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
pm = messaging.PubMaster(['liveTorqueParameters'])
|
||||
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose')
|
||||
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
|
||||
|
||||
params = Params()
|
||||
estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
|
||||
@ -254,7 +244,7 @@ def main(demo=False):
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
estimator.handle_log(t, which, sm[which])
|
||||
|
||||
# 4Hz driven by livePose
|
||||
# 4Hz driven by liveLocationKalman
|
||||
if sm.frame % 5 == 0:
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
|
||||
|
||||
|
||||
@ -48,7 +48,7 @@ LONG_SMOOTH_SECONDS = 0.3
|
||||
|
||||
|
||||
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action:
|
||||
lat_action_t: float, long_action_t: float, lat_smooth_seconds: float) -> log.ModelDataV2.Action:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
|
||||
plan[:,Plan.ACCELERATION][:,0],
|
||||
@ -57,7 +57,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
||||
|
||||
desired_curvature = model_output['desired_curvature'][0, 0]
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth_seconds)
|
||||
|
||||
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
|
||||
desiredAcceleration=float(desired_accel),
|
||||
@ -218,7 +218,7 @@ def main(demo=False):
|
||||
|
||||
# messaging
|
||||
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "carrotMan", "radarState"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "carrotMan", "radarState"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
@ -251,8 +251,19 @@ def main(demo=False):
|
||||
|
||||
DH = DesireHelper()
|
||||
|
||||
frame = 0
|
||||
custom_lat_delay = 0.0
|
||||
lat_smooth_seconds = LAT_SMOOTH_SECONDS
|
||||
while True:
|
||||
lat_delay = params.get_float("SteerActuatorDelay") * 0.01 + LAT_SMOOTH_SECONDS
|
||||
frame += 1
|
||||
if frame % 100 == 0:
|
||||
custom_lat_delay = params.get_float("SteerActuatorDelay") * 0.01
|
||||
lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01
|
||||
|
||||
if custom_lat_delay > 0.0:
|
||||
lat_delay = custom_lat_delay + lat_smooth_seconds
|
||||
else:
|
||||
lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds
|
||||
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
|
||||
@ -335,7 +346,7 @@ def main(demo=False):
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
|
||||
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL)
|
||||
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, lat_smooth_seconds)
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@ -82,7 +82,7 @@ class SelfdriveD:
|
||||
# no vipc in replay will make them ignored anyways
|
||||
ignore += ['roadCameraState', 'wideRoadCameraState']
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', # 'liveDelay',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'carrotMan',
|
||||
'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \
|
||||
@ -340,11 +340,13 @@ class SelfdriveD:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not self.CP.notCar:
|
||||
if not self.sm['livePose'].posenetOK:
|
||||
if not self.sm['liveLocationKalman'].posenetOK:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['livePose'].inputsOK:
|
||||
if not self.sm['liveLocationKalman'].deviceStable:
|
||||
self.events.add(EventName.deviceFalling)
|
||||
if not self.sm['liveLocationKalman'].inputsOK:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error
|
||||
@ -385,7 +387,7 @@ class SelfdriveD:
|
||||
if not SIMULATION or REPLAY:
|
||||
# Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
|
||||
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
|
||||
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
|
||||
if not gps_ok and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500):
|
||||
if self.distance_traveled < 1600:
|
||||
self.events.add(EventName.noGps)
|
||||
#if gps_ok:
|
||||
|
||||
@ -2209,24 +2209,11 @@ public:
|
||||
if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
|
||||
strcpy(driving_mode_str_last, driving_mode_str);
|
||||
|
||||
if(sm.updated(s->gps_location_socket)) {
|
||||
if (!strcmp(s->gps_location_socket, "gpsLocationExternal")) {
|
||||
auto gpsLocation = sm[s->gps_location_socket].getGpsLocationExternal();
|
||||
ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
|
||||
}
|
||||
else {
|
||||
auto gpsLocation = sm[s->gps_location_socket].getGpsLocation();
|
||||
ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
|
||||
|
||||
}
|
||||
}
|
||||
/*
|
||||
auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
|
||||
bool is_gps_valid = locationd.getGpsOK();
|
||||
bool is_gps_valid = sm.valid("liveLocationKalman") && locationd.getGpsOK();
|
||||
if (is_gps_valid) {
|
||||
ui_draw_text(s, dx, dy - 45, "GPS", 30, COLOR_GREEN, BOLD);
|
||||
}
|
||||
*/
|
||||
|
||||
char gap_str[32];
|
||||
int gap = params.getInt("LongitudinalPersonality") + 1;
|
||||
@ -2747,9 +2734,11 @@ public:
|
||||
str = QString::fromStdString(car_state.getLogCarrot());
|
||||
sprintf(top, "%s", str.toStdString().c_str());
|
||||
// top_right
|
||||
const auto live_delay = sm["liveDelay"].getLiveDelay();
|
||||
const auto live_torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
|
||||
const auto live_params = sm["liveParameters"].getLiveParameters();
|
||||
str.sprintf("LT[%.0f,%s](%.2f/%.2f), SR(%.1f,%.1f)",
|
||||
str.sprintf("LD[%d,%.2f],LT[%.0f,%s](%.2f/%.2f), SR(%.1f,%.1f)",
|
||||
live_delay.getValidBlocks(), live_delay.getLateralDelay(),
|
||||
live_torque_params.getTotalBucketPoints(), live_torque_params.getLiveValid() ? "ON" : "OFF",
|
||||
live_torque_params.getLatAccelFactorFiltered(), live_torque_params.getFrictionCoefficientFiltered(),
|
||||
live_params.getSteerRatio(), params.getFloat("CustomSR")/10.0);
|
||||
|
||||
@ -295,7 +295,6 @@ void MapWindow::updateState(const UIState &s) {
|
||||
updateDestinationMarker();
|
||||
}
|
||||
if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) {
|
||||
/*
|
||||
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
|
||||
if (locationd_location.getGpsOK()) {
|
||||
//auto carrot_man = sm["carrotMan"].getCarrotMan();
|
||||
@ -307,7 +306,6 @@ void MapWindow::updateState(const UIState &s) {
|
||||
modelV2Path["data"] = QVariant::fromValue<QMapLibre::Feature>(model_path_feature);
|
||||
m_map->updateSource("modelPathSource", modelV2Path);
|
||||
}
|
||||
*/
|
||||
model_rcv_frame = sm.rcv_frame("modelV2");
|
||||
}
|
||||
}
|
||||
|
||||
@ -692,7 +692,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
|
||||
latLongToggles->addItem(new CValueControl("CruiseMaxVals6", "ACCEL:140km/h(60)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5));
|
||||
//latLongToggles->addItem(new CValueControl("CruiseMinVals", "DECEL:(120)", "감속도를 설정합니다.(x0.01m/s^2)", "../assets/offroad/icon_road.png", 50, 250, 5));
|
||||
latLongToggles->addItem(new CValueControl("MaxAngleFrames", "MaxAngleFrames(89)", "89:기본, 스티어계기판에러시 85~87", "../assets/offroad/icon_logic.png", 80, 100, 1));
|
||||
latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "표준", "../assets/offroad/icon_logic.png", 1, 100, 1));
|
||||
latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "x0.01, 0:LiveDelay", "../assets/offroad/icon_logic.png", 0, 100, 1));
|
||||
latLongToggles->addItem(new CValueControl("SteerSmoothSec", "LAT:SteerSmoothSec(13)", "x0.01", "../assets/offroad/icon_logic.png", 1, 100, 1));
|
||||
latLongToggles->addItem(new CValueControl("LateralTorqueCustom", "LAT: TorqueCustom(0)", "", "../assets/offroad/icon_logic.png", 0, 2, 1));
|
||||
latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", "LAT: TorqueAccelFactor(2500)", "", "../assets/offroad/icon_logic.png", 1000, 6000, 10));
|
||||
latLongToggles->addItem(new CValueControl("LateralTorqueFriction", "LAT: TorqueFriction(100)", "", "../assets/offroad/icon_logic.png", 0, 1000, 10));
|
||||
|
||||
@ -98,20 +98,13 @@ void UIState::updateStatus() {
|
||||
}
|
||||
|
||||
UIState::UIState(QObject *parent) : QObject(parent) {
|
||||
Params params;
|
||||
if (params.getBool("UbloxAvailable")) {
|
||||
strcpy(gps_location_socket, "gpsLocationExternal");
|
||||
}
|
||||
else {
|
||||
strcpy(gps_location_socket, "gpsLocation");
|
||||
}
|
||||
sm = std::make_unique<SubMaster>(std::vector<const char*>{
|
||||
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
|
||||
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
|
||||
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
|
||||
"longitudinalPlan",
|
||||
"carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
|
||||
"navRoute", "navInstruction", "navInstructionCarrot", gps_location_socket,
|
||||
"navRoute", "navInstruction", "navInstructionCarrot", "liveLocationKalman", "liveDelay",
|
||||
});
|
||||
prime_state = new PrimeState(this);
|
||||
language = QString::fromStdString(Params().get("LanguageSetting"));
|
||||
|
||||
@ -98,8 +98,6 @@ public:
|
||||
float show_brightness_ratio = 1.0;
|
||||
int show_brightness_timer = 20;
|
||||
|
||||
char gps_location_socket[32] = "gpsLocationExternal";
|
||||
|
||||
signals:
|
||||
void uiUpdate(const UIState &s);
|
||||
void offroadTransition(bool offroad);
|
||||
|
||||
@ -145,7 +145,8 @@ def get_default_params():
|
||||
("CustomSteerDeltaUp", "0"),
|
||||
("CustomSteerDeltaDown", "0"),
|
||||
("SpeedFromPCM", "2"),
|
||||
("SteerActuatorDelay", "30"),
|
||||
("SteerActuatorDelay", "0"),
|
||||
("SteerSmoothSec", "13"),
|
||||
("MaxTimeOffroadMin", "60"),
|
||||
("DisableDM", "0"),
|
||||
("RecordRoadCam", "0"),
|
||||
|
||||
@ -94,7 +94,8 @@ procs = [
|
||||
NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
|
||||
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
|
||||
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
|
||||
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
|
||||
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad),
|
||||
#PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
|
||||
NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
|
||||
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
|
||||
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
|
||||
@ -108,7 +109,7 @@ procs = [
|
||||
PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
|
||||
PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
|
||||
PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
|
||||
#PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
|
||||
PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
|
||||
NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
|
||||
PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
|
||||
PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),
|
||||
|
||||
@ -333,7 +333,9 @@ def main() -> NoReturn:
|
||||
pm.send('qcomGnss', msg)
|
||||
elif log_type == LOG_GNSS_POSITION_REPORT:
|
||||
report = unpack_position(log_payload)
|
||||
#print(report)
|
||||
if report["u_PosSource"] != 2:
|
||||
print("u_PosSource =", report["u_PosSource"])
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user