mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-23 23:12:04 +08:00
remove old debug scripts
This commit is contained in:
Adeeb Shihadeh
@@ -1,19 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
import cereal.messaging as messaging
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan'])
|
||||
|
||||
i = 0
|
||||
while True:
|
||||
sm.update(0)
|
||||
|
||||
i += 1
|
||||
if i % 100 == 0:
|
||||
print()
|
||||
print("alive", sm.alive)
|
||||
print("valid", sm.valid)
|
||||
|
||||
time.sleep(0.01)
|
||||
@@ -1,43 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import time
|
||||
import statistics
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from typing import Dict
|
||||
|
||||
camera_states = [
|
||||
'roadCameraState',
|
||||
'wideRoadCameraState',
|
||||
'driverCameraState'
|
||||
]
|
||||
|
||||
def fmt(val):
|
||||
ref = 0.05
|
||||
return f"{val:.6f} ({100 * val / ref:.2f}%)"
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(camera_states)
|
||||
|
||||
prev_sof = {state: None for state in camera_states}
|
||||
diffs: Dict[str, list] = {state: [] for state in camera_states}
|
||||
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
for state in camera_states:
|
||||
if sm.updated[state]:
|
||||
if prev_sof[state] is not None:
|
||||
diffs[state].append((sm[state].timestampSof - prev_sof[state]) / 1e9)
|
||||
prev_sof[state] = sm[state].timestampSof
|
||||
|
||||
if time.monotonic() - st > 10:
|
||||
for state in camera_states:
|
||||
values = diffs[state]
|
||||
ref = 0.05
|
||||
print(f"{state} \tMean: {fmt(statistics.mean(values))} \t Min: {fmt(min(values))} \t Max: {fmt(max(values))} \t Std: {statistics.stdev(values):.6f} \t Num frames: {len(values)}")
|
||||
diffs[state] = []
|
||||
|
||||
print()
|
||||
st = time.monotonic()
|
||||
@@ -1,32 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
import control # pylint: disable=import-error
|
||||
|
||||
dt = 0.01
|
||||
A = np.array([[ 0. , 1. ], [-0.78823806, 1.78060701]])
|
||||
B = np.array([[-2.23399437e-05], [ 7.58330763e-08]])
|
||||
C = np.array([[1., 0.]])
|
||||
|
||||
|
||||
# Kalman tuning
|
||||
Q = np.diag([1, 1])
|
||||
R = np.atleast_2d(1e5)
|
||||
|
||||
(_, _, L) = control.dare(A.T, C.T, Q, R)
|
||||
L = L.T
|
||||
|
||||
# LQR tuning
|
||||
Q = np.diag([2e5, 1e-5])
|
||||
R = np.atleast_2d(1)
|
||||
(_, _, K) = control.dare(A, B, Q, R)
|
||||
|
||||
A_cl = (A - B.dot(K))
|
||||
sys = control.ss(A_cl, B, C, 0, dt)
|
||||
dc_gain = control.dcgain(sys)
|
||||
|
||||
print(("self.A = np." + A.__repr__()).replace('\n', ''))
|
||||
print(("self.B = np." + B.__repr__()).replace('\n', ''))
|
||||
print(("self.C = np." + C.__repr__()).replace('\n', ''))
|
||||
print(("self.K = np." + K.__repr__()).replace('\n', ''))
|
||||
print(("self.L = np." + L.__repr__()).replace('\n', ''))
|
||||
print("self.dc_gain = " + str(dc_gain))
|
||||
@@ -1,64 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import sys
|
||||
from datetime import datetime
|
||||
|
||||
def average(avg, sample):
|
||||
# Weighted avg between existing value and new sample
|
||||
return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
start_time = datetime.now()
|
||||
try:
|
||||
if len(sys.argv) > 1 and sys.argv[1] == "--charge":
|
||||
print("not disabling charging")
|
||||
else:
|
||||
print("disabling charging")
|
||||
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
|
||||
|
||||
voltage_average = (0., 0) # average, count
|
||||
current_average = (0., 0)
|
||||
power_average = (0., 0)
|
||||
capacity_average = (0., 0)
|
||||
bat_temp_average = (0., 0)
|
||||
while 1:
|
||||
with open("/sys/class/power_supply/bms/voltage_now") as f:
|
||||
voltage = int(f.read()) / 1e6 # volts
|
||||
|
||||
with open("/sys/class/power_supply/bms/current_now") as f:
|
||||
current = int(f.read()) / 1e3 # ma
|
||||
|
||||
power = voltage * current
|
||||
|
||||
with open("/sys/class/power_supply/bms/capacity_raw") as f:
|
||||
capacity = int(f.read()) / 1e2 # percent
|
||||
|
||||
with open("/sys/class/power_supply/bms/temp") as f:
|
||||
bat_temp = int(f.read()) / 1e1 # celsius
|
||||
|
||||
# compute averages
|
||||
voltage_average = average(voltage_average, voltage)
|
||||
current_average = average(current_average, current)
|
||||
power_average = average(power_average, power)
|
||||
capacity_average = average(capacity_average, capacity)
|
||||
bat_temp_average = average(bat_temp_average, bat_temp)
|
||||
|
||||
print(f"{voltage:.2f} volts {current:12.2f} ma {power:12.2f} mW {capacity:8.2f}% battery {bat_temp:8.1f} degC")
|
||||
time.sleep(0.1)
|
||||
finally:
|
||||
stop_time = datetime.now()
|
||||
print("\n----------------------Average-----------------------------------")
|
||||
voltage = voltage_average[0]
|
||||
current = current_average[0]
|
||||
power = power_average[0]
|
||||
capacity = capacity_average[0]
|
||||
bat_temp = bat_temp_average[0]
|
||||
print(f"{voltage:.2f} volts {current:12.2f} ma {power:12.2f} mW {capacity:8.2f}% battery {bat_temp:8.1f} degC")
|
||||
print(f" {(stop_time - start_time).total_seconds():.2f} Seconds {voltage_average[1]} samples")
|
||||
print("----------------------------------------------------------------")
|
||||
|
||||
# re-enable charging
|
||||
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
|
||||
print("charging enabled\n")
|
||||
@@ -1,150 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# pylint: skip-file
|
||||
# flake8: noqa
|
||||
# type: ignore
|
||||
|
||||
import math
|
||||
import multiprocessing
|
||||
|
||||
import numpy as np
|
||||
from tqdm import tqdm
|
||||
|
||||
from selfdrive.locationd.paramsd import ParamsLearner, States
|
||||
from tools.lib.logreader import LogReader
|
||||
from tools.lib.route import Route
|
||||
|
||||
ROUTE = "b2f1615665781088|2021-03-14--17-27-47"
|
||||
PLOT = True
|
||||
|
||||
|
||||
def load_segment(segment_name):
|
||||
print(f"Loading {segment_name}")
|
||||
if segment_name is None:
|
||||
return []
|
||||
|
||||
try:
|
||||
return list(LogReader(segment_name))
|
||||
except ValueError as e:
|
||||
print(f"Error parsing {segment_name}: {e}")
|
||||
return []
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
route = Route(ROUTE)
|
||||
|
||||
msgs = []
|
||||
with multiprocessing.Pool(24) as pool:
|
||||
for d in pool.map(load_segment, route.log_paths()):
|
||||
msgs += d
|
||||
|
||||
for m in msgs:
|
||||
if m.which() == 'carParams':
|
||||
CP = m.carParams
|
||||
break
|
||||
|
||||
params = {
|
||||
'carFingerprint': CP.carFingerprint,
|
||||
'steerRatio': CP.steerRatio,
|
||||
'stiffnessFactor': 1.0,
|
||||
'angleOffsetAverageDeg': 0.0,
|
||||
}
|
||||
|
||||
for m in msgs:
|
||||
if m.which() == 'liveParameters':
|
||||
params['steerRatio'] = m.liveParameters.steerRatio
|
||||
params['angleOffsetAverageDeg'] = m.liveParameters.angleOffsetAverageDeg
|
||||
break
|
||||
|
||||
for m in msgs:
|
||||
if m.which() == 'carState':
|
||||
last_carstate = m
|
||||
break
|
||||
|
||||
print(params)
|
||||
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
|
||||
msgs = [m for m in tqdm(msgs) if m.which() in ('liveLocationKalman', 'carState', 'liveParameters')]
|
||||
msgs = sorted(msgs, key=lambda m: m.logMonoTime)
|
||||
|
||||
ts = []
|
||||
ts_log = []
|
||||
results = []
|
||||
results_log = []
|
||||
for m in tqdm(msgs):
|
||||
if m.which() == 'carState':
|
||||
last_carstate = m
|
||||
|
||||
elif m.which() == 'liveLocationKalman':
|
||||
t = last_carstate.logMonoTime / 1e9
|
||||
learner.handle_log(t, 'carState', last_carstate.carState)
|
||||
|
||||
t = m.logMonoTime / 1e9
|
||||
learner.handle_log(t, 'liveLocationKalman', m.liveLocationKalman)
|
||||
|
||||
x = learner.kf.x
|
||||
sr = float(x[States.STEER_RATIO])
|
||||
st = float(x[States.STIFFNESS])
|
||||
ao_avg = math.degrees(x[States.ANGLE_OFFSET])
|
||||
ao = ao_avg + math.degrees(x[States.ANGLE_OFFSET_FAST])
|
||||
r = [sr, st, ao_avg, ao]
|
||||
if any(math.isnan(v) for v in r):
|
||||
print("NaN", t)
|
||||
|
||||
ts.append(t)
|
||||
results.append(r)
|
||||
|
||||
elif m.which() == 'liveParameters':
|
||||
t = m.logMonoTime / 1e9
|
||||
mm = m.liveParameters
|
||||
|
||||
r = [mm.steerRatio, mm.stiffnessFactor, mm.angleOffsetAverageDeg, mm.angleOffsetDeg]
|
||||
if any(math.isnan(v) for v in r):
|
||||
print("NaN in log", t)
|
||||
ts_log.append(t)
|
||||
results_log.append(r)
|
||||
|
||||
results = np.asarray(results)
|
||||
results_log = np.asarray(results_log)
|
||||
|
||||
if PLOT:
|
||||
import matplotlib.pyplot as plt
|
||||
plt.figure()
|
||||
|
||||
plt.subplot(3, 2, 1)
|
||||
plt.plot(ts, results[:, 0], label='Steer Ratio')
|
||||
plt.grid()
|
||||
plt.ylim([0, 20])
|
||||
plt.legend()
|
||||
|
||||
plt.subplot(3, 2, 3)
|
||||
plt.plot(ts, results[:, 1], label='Stiffness')
|
||||
plt.ylim([0, 2])
|
||||
plt.grid()
|
||||
plt.legend()
|
||||
|
||||
plt.subplot(3, 2, 5)
|
||||
plt.plot(ts, results[:, 2], label='Angle offset (average)')
|
||||
plt.plot(ts, results[:, 3], label='Angle offset (instant)')
|
||||
plt.ylim([-5, 5])
|
||||
plt.grid()
|
||||
plt.legend()
|
||||
|
||||
plt.subplot(3, 2, 2)
|
||||
plt.plot(ts_log, results_log[:, 0], label='Steer Ratio')
|
||||
plt.grid()
|
||||
plt.ylim([0, 20])
|
||||
plt.legend()
|
||||
|
||||
plt.subplot(3, 2, 4)
|
||||
plt.plot(ts_log, results_log[:, 1], label='Stiffness')
|
||||
plt.ylim([0, 2])
|
||||
plt.grid()
|
||||
plt.legend()
|
||||
|
||||
plt.subplot(3, 2, 6)
|
||||
plt.plot(ts_log, results_log[:, 2], label='Angle offset (average)')
|
||||
plt.plot(ts_log, results_log[:, 3], label='Angle offset (instant)')
|
||||
plt.ylim([-5, 5])
|
||||
plt.grid()
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
@@ -1,133 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# pylint: skip-file
|
||||
# type: ignore
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
from tqdm import tqdm
|
||||
from typing import cast
|
||||
|
||||
import seaborn as sns
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
from selfdrive.car.honda.values import CAR
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel, create_dyn_state_matrices
|
||||
from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
|
||||
|
||||
T_SIM = 5 * 60 # s
|
||||
DT = 0.01
|
||||
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.CIVIC)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
x, y = 0, 0 # m, m
|
||||
psi = math.radians(0) # rad
|
||||
|
||||
# The state is x = [v, r]^T
|
||||
# with v lateral speed [m/s], and r rotational speed [rad/s]
|
||||
state = np.array([[0.0], [0.0]])
|
||||
|
||||
|
||||
ts = np.arange(0, T_SIM, DT)
|
||||
speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25
|
||||
|
||||
angle_offsets = math.radians(1.0) * np.ones_like(ts)
|
||||
angle_offsets[ts > 60] = 0
|
||||
steering_angles = cast(np.ndarray, np.radians(5 * np.cos(2 * np.pi * ts / 100.)))
|
||||
|
||||
xs = []
|
||||
ys = []
|
||||
psis = []
|
||||
yaw_rates = []
|
||||
speed_ys = []
|
||||
|
||||
|
||||
kf_states = []
|
||||
kf_ps = []
|
||||
|
||||
kf = CarKalman()
|
||||
|
||||
for i, t in tqdm(list(enumerate(ts))):
|
||||
u = speeds[i]
|
||||
sa = steering_angles[i]
|
||||
ao = angle_offsets[i]
|
||||
|
||||
A, B = create_dyn_state_matrices(u, VM)
|
||||
|
||||
state += DT * (A.dot(state) + B.dot(sa + ao))
|
||||
|
||||
x += u * math.cos(psi) * DT
|
||||
y += (float(state[0]) * math.sin(psi) + u * math.sin(psi)) * DT
|
||||
psi += float(state[1]) * DT
|
||||
|
||||
kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [float(state[1])])
|
||||
kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[u, float(state[0])]])
|
||||
kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [sa])
|
||||
kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0])
|
||||
kf.predict(t)
|
||||
|
||||
speed_ys.append(float(state[0]))
|
||||
yaw_rates.append(float(state[1]))
|
||||
kf_states.append(kf.x.copy())
|
||||
kf_ps.append(kf.P.copy())
|
||||
|
||||
xs.append(x)
|
||||
ys.append(y)
|
||||
psis.append(psi)
|
||||
|
||||
|
||||
xs = np.asarray(xs)
|
||||
ys = np.asarray(ys)
|
||||
psis = np.asarray(psis)
|
||||
speed_ys = np.asarray(speed_ys)
|
||||
kf_states = np.asarray(kf_states)
|
||||
kf_ps = np.asarray(kf_ps)
|
||||
|
||||
|
||||
palette = sns.color_palette()
|
||||
|
||||
def plot_with_bands(ts, state, label, ax, idx=1, converter=None):
|
||||
mean = kf_states[:, state].flatten()
|
||||
stds = np.sqrt(kf_ps[:, state, state].flatten())
|
||||
|
||||
if converter is not None:
|
||||
mean = converter(mean)
|
||||
stds = converter(stds)
|
||||
|
||||
sns.lineplot(ts, mean, label=label, ax=ax)
|
||||
ax.fill_between(ts, mean - stds, mean + stds, alpha=.2, color=palette[idx])
|
||||
|
||||
|
||||
print(kf.x)
|
||||
|
||||
sns.set_context("paper")
|
||||
f, axes = plt.subplots(6, 1)
|
||||
|
||||
sns.lineplot(ts, np.degrees(steering_angles), label='Steering Angle [deg]', ax=axes[0])
|
||||
plot_with_bands(ts, States.STEER_ANGLE, 'Steering Angle kf [deg]', axes[0], converter=np.degrees)
|
||||
|
||||
sns.lineplot(ts, np.degrees(yaw_rates), label='Yaw Rate [deg]', ax=axes[1])
|
||||
plot_with_bands(ts, States.YAW_RATE, 'Yaw Rate kf [deg]', axes[1], converter=np.degrees)
|
||||
|
||||
sns.lineplot(ts, np.ones_like(ts) * VM.sR, label='Steer ratio [-]', ax=axes[2])
|
||||
plot_with_bands(ts, States.STEER_RATIO, 'Steer ratio kf [-]', axes[2])
|
||||
axes[2].set_ylim([10, 20])
|
||||
|
||||
|
||||
sns.lineplot(ts, np.ones_like(ts), label='Tire stiffness[-]', ax=axes[3])
|
||||
plot_with_bands(ts, States.STIFFNESS, 'Tire stiffness kf [-]', axes[3])
|
||||
axes[3].set_ylim([0.8, 1.2])
|
||||
|
||||
|
||||
sns.lineplot(ts, np.degrees(angle_offsets), label='Angle offset [deg]', ax=axes[4])
|
||||
plot_with_bands(ts, States.ANGLE_OFFSET, 'Angle offset kf deg', axes[4], converter=np.degrees)
|
||||
plot_with_bands(ts, States.ANGLE_OFFSET_FAST, 'Fast Angle offset kf deg', axes[4], converter=np.degrees, idx=2)
|
||||
|
||||
axes[4].set_ylim([-2, 2])
|
||||
|
||||
sns.lineplot(ts, speeds, ax=axes[5])
|
||||
|
||||
plt.show()
|
||||
Reference in New Issue
Block a user