diff --git a/selfdrive/debug/internal/check_alive_valid.py b/selfdrive/debug/internal/check_alive_valid.py deleted file mode 100755 index da488c2140..0000000000 --- a/selfdrive/debug/internal/check_alive_valid.py +++ /dev/null @@ -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) diff --git a/selfdrive/debug/internal/check_frame_frequencies.py b/selfdrive/debug/internal/check_frame_frequencies.py deleted file mode 100755 index 9ac8bfc80e..0000000000 --- a/selfdrive/debug/internal/check_frame_frequencies.py +++ /dev/null @@ -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() diff --git a/selfdrive/debug/internal/design_lqr.py b/selfdrive/debug/internal/design_lqr.py deleted file mode 100755 index e86926f607..0000000000 --- a/selfdrive/debug/internal/design_lqr.py +++ /dev/null @@ -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)) diff --git a/selfdrive/debug/internal/power_monitor.py b/selfdrive/debug/internal/power_monitor.py deleted file mode 100755 index 34d2a12adc..0000000000 --- a/selfdrive/debug/internal/power_monitor.py +++ /dev/null @@ -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") diff --git a/selfdrive/debug/internal/run_paramsd_on_route.py b/selfdrive/debug/internal/run_paramsd_on_route.py deleted file mode 100755 index 54519e3777..0000000000 --- a/selfdrive/debug/internal/run_paramsd_on_route.py +++ /dev/null @@ -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() - diff --git a/selfdrive/debug/internal/test_paramsd.py b/selfdrive/debug/internal/test_paramsd.py deleted file mode 100755 index 81524496f7..0000000000 --- a/selfdrive/debug/internal/test_paramsd.py +++ /dev/null @@ -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()