Adeeb Shihadeh
2 years ago
parent
e883789c0e
commit
3da28d4df2
6 changed files with 0 additions and 441 deletions
@ -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() |
|
||||||
Loading…
Reference in new issue