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