diff --git a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py index 51e38112f..547f45aa2 100644 --- a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py +++ b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py @@ -3,6 +3,7 @@ import sys import markdown import numpy as np import matplotlib.pyplot as plt +from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -108,6 +109,33 @@ def generate_mpc_tuning_report(): htmls.append(get_html_from_results(results, labels, EGO_A)) + results = {} + name = 'Following 5s (sinusoidal) oscillating lead' + labels = [] + speed = np.int64(10) + duration = float(30) + f_osc = 1. / 5 + for oscil in np.arange(0, 10, 1): + bps = DT_MDL * np.arange(int(duration / DT_MDL)) + lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps) + man = Maneuver( + '', + duration=duration, + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed), + speed_lead_values=lead_speeds, + breakpoints=bps, + ) + valid, results[oscil] = man.evaluate() + labels.append(f'{oscil} m/s oscilliation size') + + htmls.append(markdown.markdown('# ' + name)) + htmls.append(get_html_from_results(results, labels, D_REL)) + htmls.append(get_html_from_results(results, labels, EGO_V)) + htmls.append(get_html_from_results(results, labels, EGO_A)) + + results = {} name = 'Speed profile when converging to steady state lead at 30m/s' labels = []