diff --git a/tools/car_porting/examples/hkg_canfd_angle_steering.ipynb b/tools/car_porting/examples/hkg_canfd_angle_steering.ipynb index a34c4cccd4..972b32f9ea 100644 --- a/tools/car_porting/examples/hkg_canfd_angle_steering.ipynb +++ b/tools/car_porting/examples/hkg_canfd_angle_steering.ipynb @@ -1,461 +1,569 @@ -#%% -from opendbc.car import structs -from opendbc.car.hyundai.values import CAR, HyundaiFlags -from opendbc.car.hyundai.fingerprints import FW_VERSIONS - -TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) & set(CAR.with_flags(HyundaiFlags.CANFD_ANGLE_STEERING)) # CAN-FD electric vehicles only -#TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) - set(CAR.with_flags(HyundaiFlags.EV)) # CAN-FD hybrid and ICE vehicles only - -print(f"Found {len(TEST_PLATFORMS)} qualifying vehicles:") -for platform in TEST_PLATFORMS: - print(f" {platform}") -#%% -TEST_SEGMENTS = ['e1107f9d04dfb1e2/00000096--02ecca61a6'] - -#%% -import copy -import matplotlib.pyplot as plt -import numpy as np -from opendbc.can.parser import CANParser -from opendbc.car.hyundai.values import DBC -from opendbc.car.hyundai.hyundaicanfd import CanBus -from openpilot.selfdrive.pandad import can_capnp_to_list -from openpilot.tools.lib.logreader import LogReader - -# Keep both messages we're interested in -message_names = ["LKAS_ALT", "LFA_ALT"] - -# You need to define this variable if it's not already defined -# TEST_SEGMENTS = ["path/to/segment1", "path/to/segment2"] -# And platform variable if not defined -# platform = "CANFD" - -# Select one segment for testing/debugging -segment = TEST_SEGMENTS[0] # Change index as needed -#%% -# Load the segment -lr = LogReader(segment) -CP = lr.first("carParams") -if CP is None: - print(f"No carParams found in segment {segment}") -else: - print(f"Analyzing segment {segment} for {CP.carFingerprint}") - - # Get CAN messages - can_msgs = [msg for msg in lr if msg.which() == "can"] - print(f"Found {len(can_msgs)} CAN messages") - - # Setup parser - parser_messages = [] - for name in message_names: - parser_messages.append((name, 0)) - - try: - cp = CANParser(DBC[platform]["pt"], parser_messages, CanBus(CP).ECAN) - print("Parser initialized successfully") - except Exception as e: - print(f"Error initializing parser: {e}") -#%% -parser_messages = [("LKAS_ALT",0),("LFA_ALT",0)] -# Reset parser -cp = CANParser(DBC[platform]["pt"], parser_messages, CanBus(CP).ECAN) - -# Check a single message -example_idx = 0 -a = set() -while example_idx < len(can_msgs): - try: - cp.update_strings(can_capnp_to_list([can_msgs[example_idx].as_builder().to_bytes()])) - - #Print all available signals in both messages - #print("Available signals:") - #if "LKAS_ALT" in cpacan.vl: - # print(" LKAS_ALT:", cpacan.vl["LKAS_ALT"]) - #if "LFA_ALT" in cp.vl: - # print(" LFA_ALT:", cp.vl["LFA_ALT"]) - - # Found at least one message with data - #if "LKAS_ALT" in cp.vl or "LFA_ALT" in cp.vl: - # break - #if cp.vl["LFA_ALT"]["LKAS_ANGLE_MAX_TORQUE"]>0: - # a.add(cp.vl["LFA_ALT"]["LKAS_ANGLE_MAX_TORQUE"]) - #print("AAAA") - #break - if cp.vl["LFA_ALT"]["LKAS_ANGLE_ACTIVE"]>0: - a.add(cp.vl["LFA_ALT"]["LKAS_ANGLE_ACTIVE"]) - #print("AAAA") - #break - - except Exception as e: - print(f"Error examining message {example_idx}: {e}") - - example_idx += 1 - - -print(example_idx) # 72009 -print(a) -#%% -# Reset parser -cp = CANParser(DBC[platform]["pt"], parser_messages, CanBus(CP).ECAN) - -# Track timestamps and angle commands when active -timestamps = [] -angle_cmds = [] -active_flags = [] # For debugging -msg_indices = [] # Store original message indices for reference - -# Process all CAN messages -for i, msg in enumerate(can_msgs): - try: - cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()])) - - # Check if both required messages and signals are present - if "LFA_ALT" in cp.vl and "LKAS_ALT" in cp.vl: - if "LKAS_ANGLE_ACTIVE" in cp.vl["LFA_ALT"] and "LKAS_ANGLE_CMD" in cp.vl["LKAS_ALT"]: - active_val = cp.vl["LFA_ALT"]["LKAS_ANGLE_ACTIVE"] - active_flags.append(active_val) - - # Only collect data when active is 2 - if active_val == 2: - timestamps.append(msg.logMonoTime / 1e9) # Convert to seconds - angle_cmds.append(cp.vl["LFA_ALT"]["LKAS_ANGLE_CMD"]) - msg_indices.append(i) - except Exception as e: - if i % 1000 == 0: # Only print errors occasionally to avoid flooding - print(f"Error processing message {i}: {e}") - -print(f"Total messages processed: {len(can_msgs)}") -print(f"LKAS_ANGLE_ACTIVE values encountered: {set(active_flags)}") -print(f"Active steering points collected: {len(timestamps)}") -#%% -# Find continuous active periods -active_periods = [] -if msg_indices: - # Start with the first active message - current_period = [0, 0] - - for i in range(1, len(msg_indices)): - # Check if consecutive in the original message sequence - if msg_indices[i] == msg_indices[i-1] + 1: - # Part of the same period - current_period[1] = i - else: - # Gap in sequence, start new period - active_periods.append(current_period) - current_period = [i, i] - - # Add the last period - active_periods.append(current_period) - -print(f"Found {len(active_periods)} active steering periods") -for i, (start, end) in enumerate(active_periods): - duration = timestamps[end] - timestamps[start] - num_points = end - start + 1 - print(f" Period {i+1}: {num_points} points over {duration:.2f}s") -#%% -## Choose a period to analyze (if any exist) -if active_periods: - # Find the period with the most points - #longest_period_idx = max(range(len(active_periods)), - # key=lambda i: active_periods[i][1] - active_periods[i][0]) - longest_period_idx = 1 - - start_idx, end_idx = active_periods[longest_period_idx] - - # Get data for this period - period_timestamps = timestamps[start_idx:end_idx+1] - period_angle_cmds = angle_cmds[start_idx:end_idx+1] - - # Normalize timestamps to start at 0 - norm_timestamps = [t - period_timestamps[0] for t in period_timestamps] - - # Calculate rate of change - angle_cmd_diffs = np.diff(period_angle_cmds) - - # Print statistics - print(f"Analysis of Period {longest_period_idx+1}:") - print(f" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s") - print(f" Points: {len(period_angle_cmds)}") - print(f" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}") - print(f" Max absolute rate of change: {max(abs(angle_cmd_diffs)):.2f} units/frame") - - # Plot the angle command evolution - plt.figure(figsize=(12, 8)) - - # Plot angle commands - plt.subplot(2, 1, 1) - plt.plot(norm_timestamps, period_angle_cmds) - plt.title(f"LKAS_ANGLE_CMD Evolution - Period {longest_period_idx+1}") - plt.xlabel("Time (s)") - plt.ylabel("LKAS_ANGLE_CMD") - plt.grid(True) - - # Plot rate of change - plt.subplot(2, 1, 2) - plt.plot(norm_timestamps[1:], angle_cmd_diffs) - plt.title("Rate of Change") - plt.xlabel("Time (s)") - plt.ylabel("Delta LKAS_ANGLE_CMD per frame") - plt.grid(True) - - plt.tight_layout() - plt.show() -else: - print("No active periods found to analyze") -#%% -# Analyze all active periods -if active_periods: - # Initialize variables to track overall statistics - all_max_rates = [] - - # Create a figure with subplots for all periods - num_periods = len(active_periods) - fig, axes = plt.subplots(num_periods, 2, figsize=(14, 5*num_periods)) - - # If there's only one period, make axes indexable as a 2D array - if num_periods == 1: - axes = np.array([axes]) - - # Process each active period - for i, (start_idx, end_idx) in enumerate(active_periods): - # Get data for this period - period_timestamps = timestamps[start_idx:end_idx+1] - period_angle_cmds = angle_cmds[start_idx:end_idx+1] - - # Normalize timestamps to start at 0 - norm_timestamps = [t - period_timestamps[0] for t in period_timestamps] - - # Calculate rate of change - angle_cmd_diffs = np.diff(period_angle_cmds) - max_rate = max(abs(angle_cmd_diffs)) - all_max_rates.append(max_rate) - - # Print statistics for this period - print(f"Analysis of Period {i+1}:") - print(f" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s") - print(f" Points: {len(period_angle_cmds)}") - print(f" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}") - print(f" Max absolute rate of change: {max_rate:.2f} units/frame") - print() - - # Plot angle commands - axes[i, 0].plot(norm_timestamps, period_angle_cmds) - axes[i, 0].set_title(f"LKAS_ANGLE_CMD Evolution - Period {i+1}") - axes[i, 0].set_xlabel("Time (s)") - axes[i, 0].set_ylabel("LKAS_ANGLE_CMD") - axes[i, 0].grid(True) - - # Plot rate of change - axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs) - axes[i, 1].set_title(f"Rate of Change - Period {i+1}") - axes[i, 1].set_xlabel("Time (s)") - axes[i, 1].set_ylabel("Delta LKAS_ANGLE_CMD per frame") - axes[i, 1].grid(True) - - # Print average maximum rate of change - avg_max_rate = sum(all_max_rates) / len(all_max_rates) - print(f"Average maximum rate of change across all periods: {avg_max_rate:.2f} units/frame") - - # Add a summary section with all periods on one plot - plt.figure(figsize=(14, 8)) - - # Plot all periods with different colors - for i, (start_idx, end_idx) in enumerate(active_periods): - period_timestamps = timestamps[start_idx:end_idx+1] - period_angle_cmds = angle_cmds[start_idx:end_idx+1] - norm_timestamps = [t - period_timestamps[0] for t in period_timestamps] - - plt.plot(norm_timestamps, period_angle_cmds, label=f"Period {i+1}") - - plt.title("LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)") - plt.xlabel("Time (s)") - plt.ylabel("LKAS_ANGLE_CMD") - plt.grid(True) - plt.legend() - - plt.tight_layout() - plt.show() - - # Also display the original figure with individual period plots - fig.tight_layout() - plt.show() -else: - print("No active periods found to analyze") -#%% -# Analyze all active periods with enhanced metrics -if active_periods: - # Initialize variables to track overall statistics - all_max_rates_per_frame = [] - all_max_rates_per_second = [] - all_min_to_max_times = [] - all_min_to_max_rates = [] - - # Create a figure with subplots for all periods - num_periods = len(active_periods) - fig, axes = plt.subplots(num_periods, 3, figsize=(18, 6*num_periods)) - - # If there's only one period, make axes indexable as a 2D array - if num_periods == 1: - axes = np.array([axes]).reshape(1, 3) - - # Process each active period - for i, (start_idx, end_idx) in enumerate(active_periods): - # Get data for this period - period_timestamps = timestamps[start_idx:end_idx+1] - period_angle_cmds = angle_cmds[start_idx:end_idx+1] - - # Normalize timestamps to start at 0 - norm_timestamps = [t - period_timestamps[0] for t in period_timestamps] - - # Calculate rate of change per frame - angle_cmd_diffs = np.diff(period_angle_cmds) - max_rate_per_frame = max(abs(angle_cmd_diffs)) - all_max_rates_per_frame.append(max_rate_per_frame) - - # Calculate rate of change per second - time_diffs = np.diff(period_timestamps) - rates_per_second = [diff/t_diff if t_diff > 0 else 0 for diff, t_diff in zip(angle_cmd_diffs, time_diffs)] - max_rate_per_second = max(abs(rate) for rate in rates_per_second) - all_max_rates_per_second.append(max_rate_per_second) - - # Find local minima and maxima - from scipy.signal import find_peaks - - # Find local maxima - max_peaks, _ = find_peaks(period_angle_cmds) - # Find local minima (by finding maxima of negative values) - min_peaks, _ = find_peaks([-x for x in period_angle_cmds]) - - min_to_max_times = [] - min_to_max_rates = [] - - # For each minimum, find the next maximum and calculate metrics - for min_idx in min_peaks: - # Find the next maximum after this minimum - next_max_indices = [idx for idx in max_peaks if idx > min_idx] - if next_max_indices: # If there's a maximum after this minimum - next_max_idx = next_max_indices[0] - - # Time from min to max - time_diff = period_timestamps[next_max_idx] - period_timestamps[min_idx] - - # Change in angle - angle_diff = period_angle_cmds[next_max_idx] - period_angle_cmds[min_idx] - - # Rate of change - rate = angle_diff / time_diff if time_diff > 0 else 0 - - min_to_max_times.append(time_diff) - min_to_max_rates.append(rate) - - if min_to_max_times: # If we found min-to-max transitions - avg_min_to_max_time = sum(min_to_max_times) / len(min_to_max_times) - avg_min_to_max_rate = sum(min_to_max_rates) / len(min_to_max_rates) - max_min_to_max_rate = max(abs(rate) for rate in min_to_max_rates) - - all_min_to_max_times.extend(min_to_max_times) - all_min_to_max_rates.extend(min_to_max_rates) - else: - avg_min_to_max_time = 0 - avg_min_to_max_rate = 0 - max_min_to_max_rate = 0 - - # Print statistics for this period - print(f"Analysis of Period {i+1}:") - print(f" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s") - print(f" Points: {len(period_angle_cmds)}") - print(f" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}") - print(f" Max absolute rate of change: {max_rate_per_frame:.2f} degrees/frame") - print(f" Max absolute rate of change: {max_rate_per_second:.2f} degrees/second") - if min_to_max_times: - print(f" Avg time from min to max: {avg_min_to_max_time:.2f}s") - print(f" Avg rate from min to max: {avg_min_to_max_rate:.2f} degrees/second") - print(f" Max rate from min to max: {max_min_to_max_rate:.2f} degrees/second") - print() - - # Plot angle commands - axes[i, 0].plot(norm_timestamps, period_angle_cmds) - # Add markers for mins and maxs - for min_idx in min_peaks: - axes[i, 0].plot(norm_timestamps[min_idx], period_angle_cmds[min_idx], 'rv', markersize=8) - for max_idx in max_peaks: - axes[i, 0].plot(norm_timestamps[max_idx], period_angle_cmds[max_idx], 'g^', markersize=8) - - axes[i, 0].set_title(f"LKAS_ANGLE_CMD Evolution - Period {i+1}") - axes[i, 0].set_xlabel("Time (s)") - axes[i, 0].set_ylabel("LKAS_ANGLE_CMD (degrees)") - axes[i, 0].grid(True) - - # Plot rate of change per frame - axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs) - axes[i, 1].set_title(f"Rate of Change - Period {i+1}") - axes[i, 1].set_xlabel("Time (s)") - axes[i, 1].set_ylabel("Delta (degrees/frame)") - axes[i, 1].grid(True) - - # Plot rate of change per second - axes[i, 2].plot([norm_timestamps[j] for j in range(1, len(norm_timestamps))], rates_per_second) - axes[i, 2].set_title(f"Rate of Change - Period {i+1}") - axes[i, 2].set_xlabel("Time (s)") - axes[i, 2].set_ylabel("Delta (degrees/second)") - axes[i, 2].grid(True) - - # Calculate overall statistics - avg_max_rate_per_frame = sum(all_max_rates_per_frame) / len(all_max_rates_per_frame) - avg_max_rate_per_second = sum(all_max_rates_per_second) / len(all_max_rates_per_second) - - print("\nOverall Statistics:") - print(f"Average maximum rate of change: {avg_max_rate_per_frame:.2f} degrees/frame") - print(f"Average maximum rate of change: {avg_max_rate_per_second:.2f} degrees/second") - - if all_min_to_max_times: - avg_min_to_max_time = sum(all_min_to_max_times) / len(all_min_to_max_times) - avg_min_to_max_rate = sum(all_min_to_max_rates) / len(all_min_to_max_rates) - max_min_to_max_rate = max(abs(rate) for rate in all_min_to_max_rates) - - print(f"Average time from min to max: {avg_min_to_max_time:.2f}s") - print(f"Average rate from min to max: {avg_min_to_max_rate:.2f} degrees/second") - print(f"Maximum rate from min to max: {max_min_to_max_rate:.2f} degrees/second") - - # Add a summary section with all periods on one plot - plt.figure(figsize=(14, 8)) - - # Plot all periods with different colors - for i, (start_idx, end_idx) in enumerate(active_periods): - period_timestamps = timestamps[start_idx:end_idx+1] - period_angle_cmds = angle_cmds[start_idx:end_idx+1] - norm_timestamps = [t - period_timestamps[0] for t in period_timestamps] - - plt.plot(norm_timestamps, period_angle_cmds, label=f"Period {i+1}") - - plt.title("LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)") - plt.xlabel("Time (s)") - plt.ylabel("LKAS_ANGLE_CMD (degrees)") - plt.grid(True) - plt.legend() - - # Create a histogram of min-to-max times - if all_min_to_max_times: - plt.figure(figsize=(10, 6)) - plt.hist(all_min_to_max_times, bins=15) - plt.title("Histogram of Min-to-Max Transition Times") - plt.xlabel("Time (s)") - plt.ylabel("Frequency") - plt.grid(True) - - # Create a histogram of min-to-max rates - plt.figure(figsize=(10, 6)) - plt.hist(all_min_to_max_rates, bins=15) - plt.title("Histogram of Min-to-Max Transition Rates") - plt.xlabel("Rate (degrees/second)") - plt.ylabel("Frequency") - plt.grid(True) - - plt.tight_layout() - plt.show() - - # Also display the original figure with individual period plots - fig.tight_layout() - plt.show() -else: - print("No active periods found to analyze") +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "228a6736-de31-4255-9d72-a6ff391b968d", + "metadata": { + "jupyter": { + "is_executing": true + } + }, + "outputs": [], + "source": [ + "from opendbc.car import structs\n", + "from opendbc.car.hyundai.values import CAR, HyundaiFlags\n", + "from opendbc.car.hyundai.fingerprints import FW_VERSIONS\n", + "\n", + "TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) & set(CAR.with_flags(HyundaiFlags.CANFD_ANGLE_STEERING)) # CAN-FD electric vehicles only\n", + "#TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) - set(CAR.with_flags(HyundaiFlags.EV)) # CAN-FD hybrid and ICE vehicles only\n", + "\n", + "print(f\"Found {len(TEST_PLATFORMS)} qualifying vehicles:\")\n", + "for platform in TEST_PLATFORMS:\n", + " print(f\" {platform}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ed1c8aec-c274-4c61-b83d-711ea194bf86", + "metadata": {}, + "outputs": [], + "source": [ + "TEST_SEGMENTS = ['e1107f9d04dfb1e2/00000096--02ecca61a6']\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0c75e8f2-4f5f-4f89-b8db-5223a6534a9f", + "metadata": {}, + "outputs": [], + "source": [ + "import copy\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "from opendbc.can.parser import CANParser\n", + "from opendbc.car.hyundai.values import DBC\n", + "from opendbc.car.hyundai.hyundaicanfd import CanBus\n", + "from openpilot.selfdrive.pandad import can_capnp_to_list\n", + "from openpilot.tools.lib.logreader import LogReader\n", + "\n", + "# Keep both messages we're interested in\n", + "message_names = [\"LKAS_ALT\", \"LFA_ALT\"]\n", + "\n", + "# You need to define this variable if it's not already defined\n", + "# TEST_SEGMENTS = [\"path/to/segment1\", \"path/to/segment2\"]\n", + "# And platform variable if not defined\n", + "# platform = \"CANFD\"\n", + "\n", + "# Select one segment for testing/debugging\n", + "segment = TEST_SEGMENTS[0] # Change index as needed" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7724dd97-f62e-4fd3-9f64-63d49be669d2", + "metadata": {}, + "outputs": [], + "source": [ + "# Load the segment\n", + "lr = LogReader(segment)\n", + "CP = lr.first(\"carParams\")\n", + "if CP is None:\n", + " print(f\"No carParams found in segment {segment}\")\n", + "else:\n", + " print(f\"Analyzing segment {segment} for {CP.carFingerprint}\")\n", + "\n", + " # Get CAN messages\n", + " can_msgs = [msg for msg in lr if msg.which() == \"can\"]\n", + " print(f\"Found {len(can_msgs)} CAN messages\")\n", + " \n", + " # Setup parser\n", + " parser_messages = []\n", + " for name in message_names:\n", + " parser_messages.append((name, 0))\n", + " \n", + " try:\n", + " cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n", + " print(\"Parser initialized successfully\")\n", + " except Exception as e:\n", + " print(f\"Error initializing parser: {e}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b9e0f5dd-4609-4fb2-8b36-28359256f6f6", + "metadata": {}, + "outputs": [], + "source": [ + "parser_messages = [(\"LKAS_ALT\",0),(\"LFA_ALT\",0)]\n", + "# Reset parser\n", + "cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n", + "\n", + "# Check a single message\n", + "example_idx = 0\n", + "a = set()\n", + "while example_idx < len(can_msgs):\n", + " try:\n", + " cp.update_strings(can_capnp_to_list([can_msgs[example_idx].as_builder().to_bytes()]))\n", + " \n", + " #Print all available signals in both messages\n", + " #print(\"Available signals:\")\n", + " #if \"LKAS_ALT\" in cpacan.vl:\n", + " # print(\" LKAS_ALT:\", cpacan.vl[\"LKAS_ALT\"])\n", + " #if \"LFA_ALT\" in cp.vl:\n", + " # print(\" LFA_ALT:\", cp.vl[\"LFA_ALT\"])\n", + " \n", + " # Found at least one message with data\n", + " #if \"LKAS_ALT\" in cp.vl or \"LFA_ALT\" in cp.vl:\n", + " # break\n", + " #if cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_MAX_TORQUE\"]>0:\n", + " # a.add(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_MAX_TORQUE\"])\n", + " #print(\"AAAA\")\n", + " #break\n", + " if cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"]>0:\n", + " a.add(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"])\n", + " #print(\"AAAA\")\n", + " #break\n", + " \n", + " except Exception as e:\n", + " print(f\"Error examining message {example_idx}: {e}\")\n", + " \n", + " example_idx += 1\n", + " \n", + "\n", + "print(example_idx) # 72009\n", + "print(a)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "87b5699c-10fa-456a-9dca-138eaace34e3", + "metadata": {}, + "outputs": [], + "source": [ + "# Reset parser\n", + "cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n", + "\n", + "# Track timestamps and angle commands when active\n", + "timestamps = []\n", + "angle_cmds = []\n", + "active_flags = [] # For debugging\n", + "msg_indices = [] # Store original message indices for reference\n", + "\n", + "# Process all CAN messages\n", + "for i, msg in enumerate(can_msgs):\n", + " try:\n", + " cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n", + " \n", + " # Check if both required messages and signals are present\n", + " if \"LFA_ALT\" in cp.vl and \"LKAS_ALT\" in cp.vl:\n", + " if \"LKAS_ANGLE_ACTIVE\" in cp.vl[\"LFA_ALT\"] and \"LKAS_ANGLE_CMD\" in cp.vl[\"LKAS_ALT\"]:\n", + " active_val = cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"]\n", + " active_flags.append(active_val)\n", + " \n", + " # Only collect data when active is 2\n", + " if active_val == 2:\n", + " timestamps.append(msg.logMonoTime / 1e9) # Convert to seconds\n", + " angle_cmds.append(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_CMD\"])\n", + " msg_indices.append(i)\n", + " except Exception as e:\n", + " if i % 1000 == 0: # Only print errors occasionally to avoid flooding\n", + " print(f\"Error processing message {i}: {e}\")\n", + "\n", + "print(f\"Total messages processed: {len(can_msgs)}\")\n", + "print(f\"LKAS_ANGLE_ACTIVE values encountered: {set(active_flags)}\")\n", + "print(f\"Active steering points collected: {len(timestamps)}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f6194a2e-6bc9-49c8-befc-7ca930c07f29", + "metadata": {}, + "outputs": [], + "source": [ + "# Find continuous active periods\n", + "active_periods = []\n", + "if msg_indices:\n", + " # Start with the first active message\n", + " current_period = [0, 0]\n", + " \n", + " for i in range(1, len(msg_indices)):\n", + " # Check if consecutive in the original message sequence\n", + " if msg_indices[i] == msg_indices[i-1] + 1:\n", + " # Part of the same period\n", + " current_period[1] = i\n", + " else:\n", + " # Gap in sequence, start new period\n", + " active_periods.append(current_period)\n", + " current_period = [i, i]\n", + " \n", + " # Add the last period\n", + " active_periods.append(current_period)\n", + "\n", + "print(f\"Found {len(active_periods)} active steering periods\")\n", + "for i, (start, end) in enumerate(active_periods):\n", + " duration = timestamps[end] - timestamps[start]\n", + " num_points = end - start + 1\n", + " print(f\" Period {i+1}: {num_points} points over {duration:.2f}s\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2f5d4ca7-315e-417e-9e61-f490324e6a53", + "metadata": {}, + "outputs": [], + "source": [ + "## Choose a period to analyze (if any exist)\n", + "if active_periods:\n", + " # Find the period with the most points\n", + " #longest_period_idx = max(range(len(active_periods)), \n", + " # key=lambda i: active_periods[i][1] - active_periods[i][0])\n", + " longest_period_idx = 1\n", + " \n", + " start_idx, end_idx = active_periods[longest_period_idx]\n", + " \n", + " # Get data for this period\n", + " period_timestamps = timestamps[start_idx:end_idx+1]\n", + " period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n", + " \n", + " # Normalize timestamps to start at 0\n", + " norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n", + " \n", + " # Calculate rate of change\n", + " angle_cmd_diffs = np.diff(period_angle_cmds)\n", + " \n", + " # Print statistics\n", + " print(f\"Analysis of Period {longest_period_idx+1}:\")\n", + " print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n", + " print(f\" Points: {len(period_angle_cmds)}\")\n", + " print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n", + " print(f\" Max absolute rate of change: {max(abs(angle_cmd_diffs)):.2f} units/frame\")\n", + " \n", + " # Plot the angle command evolution\n", + " plt.figure(figsize=(12, 8))\n", + " \n", + " # Plot angle commands\n", + " plt.subplot(2, 1, 1)\n", + " plt.plot(norm_timestamps, period_angle_cmds)\n", + " plt.title(f\"LKAS_ANGLE_CMD Evolution - Period {longest_period_idx+1}\")\n", + " plt.xlabel(\"Time (s)\")\n", + " plt.ylabel(\"LKAS_ANGLE_CMD\")\n", + " plt.grid(True)\n", + " \n", + " # Plot rate of change\n", + " plt.subplot(2, 1, 2)\n", + " plt.plot(norm_timestamps[1:], angle_cmd_diffs)\n", + " plt.title(\"Rate of Change\")\n", + " plt.xlabel(\"Time (s)\")\n", + " plt.ylabel(\"Delta LKAS_ANGLE_CMD per frame\")\n", + " plt.grid(True)\n", + " \n", + " plt.tight_layout()\n", + " plt.show()\n", + "else:\n", + " print(\"No active periods found to analyze\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "33c30ed5-e8aa-44e8-9e5c-dc993d83c299", + "metadata": {}, + "outputs": [], + "source": [ + "# Analyze all active periods\n", + "if active_periods:\n", + " # Initialize variables to track overall statistics\n", + " all_max_rates = []\n", + " \n", + " # Create a figure with subplots for all periods\n", + " num_periods = len(active_periods)\n", + " fig, axes = plt.subplots(num_periods, 2, figsize=(14, 5*num_periods))\n", + " \n", + " # If there's only one period, make axes indexable as a 2D array\n", + " if num_periods == 1:\n", + " axes = np.array([axes])\n", + " \n", + " # Process each active period\n", + " for i, (start_idx, end_idx) in enumerate(active_periods):\n", + " # Get data for this period\n", + " period_timestamps = timestamps[start_idx:end_idx+1]\n", + " period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n", + " \n", + " # Normalize timestamps to start at 0\n", + " norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n", + " \n", + " # Calculate rate of change\n", + " angle_cmd_diffs = np.diff(period_angle_cmds)\n", + " max_rate = max(abs(angle_cmd_diffs))\n", + " all_max_rates.append(max_rate)\n", + " \n", + " # Print statistics for this period\n", + " print(f\"Analysis of Period {i+1}:\")\n", + " print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n", + " print(f\" Points: {len(period_angle_cmds)}\")\n", + " print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n", + " print(f\" Max absolute rate of change: {max_rate:.2f} units/frame\")\n", + " print()\n", + " \n", + " # Plot angle commands\n", + " axes[i, 0].plot(norm_timestamps, period_angle_cmds)\n", + " axes[i, 0].set_title(f\"LKAS_ANGLE_CMD Evolution - Period {i+1}\")\n", + " axes[i, 0].set_xlabel(\"Time (s)\")\n", + " axes[i, 0].set_ylabel(\"LKAS_ANGLE_CMD\")\n", + " axes[i, 0].grid(True)\n", + " \n", + " # Plot rate of change\n", + " axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs)\n", + " axes[i, 1].set_title(f\"Rate of Change - Period {i+1}\")\n", + " axes[i, 1].set_xlabel(\"Time (s)\")\n", + " axes[i, 1].set_ylabel(\"Delta LKAS_ANGLE_CMD per frame\")\n", + " axes[i, 1].grid(True)\n", + " \n", + " # Print average maximum rate of change\n", + " avg_max_rate = sum(all_max_rates) / len(all_max_rates)\n", + " print(f\"Average maximum rate of change across all periods: {avg_max_rate:.2f} units/frame\")\n", + " \n", + " # Add a summary section with all periods on one plot\n", + " plt.figure(figsize=(14, 8))\n", + " \n", + " # Plot all periods with different colors\n", + " for i, (start_idx, end_idx) in enumerate(active_periods):\n", + " period_timestamps = timestamps[start_idx:end_idx+1]\n", + " period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n", + " norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n", + " \n", + " plt.plot(norm_timestamps, period_angle_cmds, label=f\"Period {i+1}\")\n", + " \n", + " plt.title(\"LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)\")\n", + " plt.xlabel(\"Time (s)\")\n", + " plt.ylabel(\"LKAS_ANGLE_CMD\")\n", + " plt.grid(True)\n", + " plt.legend()\n", + " \n", + " plt.tight_layout()\n", + " plt.show()\n", + " \n", + " # Also display the original figure with individual period plots\n", + " fig.tight_layout()\n", + " plt.show()\n", + "else:\n", + " print(\"No active periods found to analyze\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "877702bf-0c3c-43ca-8900-c57b3c9237bc", + "metadata": {}, + "outputs": [], + "source": [ + "# Analyze all active periods with enhanced metrics\n", + "if active_periods:\n", + " # Initialize variables to track overall statistics\n", + " all_max_rates_per_frame = []\n", + " all_max_rates_per_second = []\n", + " all_min_to_max_times = []\n", + " all_min_to_max_rates = []\n", + " \n", + " # Create a figure with subplots for all periods\n", + " num_periods = len(active_periods)\n", + " fig, axes = plt.subplots(num_periods, 3, figsize=(18, 6*num_periods))\n", + " \n", + " # If there's only one period, make axes indexable as a 2D array\n", + " if num_periods == 1:\n", + " axes = np.array([axes]).reshape(1, 3)\n", + " \n", + " # Process each active period\n", + " for i, (start_idx, end_idx) in enumerate(active_periods):\n", + " # Get data for this period\n", + " period_timestamps = timestamps[start_idx:end_idx+1]\n", + " period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n", + " \n", + " # Normalize timestamps to start at 0\n", + " norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n", + " \n", + " # Calculate rate of change per frame\n", + " angle_cmd_diffs = np.diff(period_angle_cmds)\n", + " max_rate_per_frame = max(abs(angle_cmd_diffs))\n", + " all_max_rates_per_frame.append(max_rate_per_frame)\n", + " \n", + " # Calculate rate of change per second\n", + " time_diffs = np.diff(period_timestamps)\n", + " rates_per_second = [diff/t_diff if t_diff > 0 else 0 for diff, t_diff in zip(angle_cmd_diffs, time_diffs)]\n", + " max_rate_per_second = max(abs(rate) for rate in rates_per_second)\n", + " all_max_rates_per_second.append(max_rate_per_second)\n", + " \n", + " # Find local minima and maxima\n", + " from scipy.signal import find_peaks\n", + " \n", + " # Find local maxima\n", + " max_peaks, _ = find_peaks(period_angle_cmds)\n", + " # Find local minima (by finding maxima of negative values)\n", + " min_peaks, _ = find_peaks([-x for x in period_angle_cmds])\n", + " \n", + " min_to_max_times = []\n", + " min_to_max_rates = []\n", + " \n", + " # For each minimum, find the next maximum and calculate metrics\n", + " for min_idx in min_peaks:\n", + " # Find the next maximum after this minimum\n", + " next_max_indices = [idx for idx in max_peaks if idx > min_idx]\n", + " if next_max_indices: # If there's a maximum after this minimum\n", + " next_max_idx = next_max_indices[0]\n", + " \n", + " # Time from min to max\n", + " time_diff = period_timestamps[next_max_idx] - period_timestamps[min_idx]\n", + " \n", + " # Change in angle\n", + " angle_diff = period_angle_cmds[next_max_idx] - period_angle_cmds[min_idx]\n", + " \n", + " # Rate of change\n", + " rate = angle_diff / time_diff if time_diff > 0 else 0\n", + " \n", + " min_to_max_times.append(time_diff)\n", + " min_to_max_rates.append(rate)\n", + " \n", + " if min_to_max_times: # If we found min-to-max transitions\n", + " avg_min_to_max_time = sum(min_to_max_times) / len(min_to_max_times)\n", + " avg_min_to_max_rate = sum(min_to_max_rates) / len(min_to_max_rates)\n", + " max_min_to_max_rate = max(abs(rate) for rate in min_to_max_rates)\n", + " \n", + " all_min_to_max_times.extend(min_to_max_times)\n", + " all_min_to_max_rates.extend(min_to_max_rates)\n", + " else:\n", + " avg_min_to_max_time = 0\n", + " avg_min_to_max_rate = 0\n", + " max_min_to_max_rate = 0\n", + " \n", + " # Print statistics for this period\n", + " print(f\"Analysis of Period {i+1}:\")\n", + " print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n", + " print(f\" Points: {len(period_angle_cmds)}\")\n", + " print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n", + " print(f\" Max absolute rate of change: {max_rate_per_frame:.2f} degrees/frame\")\n", + " print(f\" Max absolute rate of change: {max_rate_per_second:.2f} degrees/second\")\n", + " if min_to_max_times:\n", + " print(f\" Avg time from min to max: {avg_min_to_max_time:.2f}s\")\n", + " print(f\" Avg rate from min to max: {avg_min_to_max_rate:.2f} degrees/second\")\n", + " print(f\" Max rate from min to max: {max_min_to_max_rate:.2f} degrees/second\")\n", + " print()\n", + " \n", + " # Plot angle commands\n", + " axes[i, 0].plot(norm_timestamps, period_angle_cmds)\n", + " # Add markers for mins and maxs\n", + " for min_idx in min_peaks:\n", + " axes[i, 0].plot(norm_timestamps[min_idx], period_angle_cmds[min_idx], 'rv', markersize=8)\n", + " for max_idx in max_peaks:\n", + " axes[i, 0].plot(norm_timestamps[max_idx], period_angle_cmds[max_idx], 'g^', markersize=8)\n", + " \n", + " axes[i, 0].set_title(f\"LKAS_ANGLE_CMD Evolution - Period {i+1}\")\n", + " axes[i, 0].set_xlabel(\"Time (s)\")\n", + " axes[i, 0].set_ylabel(\"LKAS_ANGLE_CMD (degrees)\")\n", + " axes[i, 0].grid(True)\n", + " \n", + " # Plot rate of change per frame\n", + " axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs)\n", + " axes[i, 1].set_title(f\"Rate of Change - Period {i+1}\")\n", + " axes[i, 1].set_xlabel(\"Time (s)\")\n", + " axes[i, 1].set_ylabel(\"Delta (degrees/frame)\")\n", + " axes[i, 1].grid(True)\n", + " \n", + " # Plot rate of change per second\n", + " axes[i, 2].plot([norm_timestamps[j] for j in range(1, len(norm_timestamps))], rates_per_second)\n", + " axes[i, 2].set_title(f\"Rate of Change - Period {i+1}\")\n", + " axes[i, 2].set_xlabel(\"Time (s)\")\n", + " axes[i, 2].set_ylabel(\"Delta (degrees/second)\")\n", + " axes[i, 2].grid(True)\n", + " \n", + " # Calculate overall statistics\n", + " avg_max_rate_per_frame = sum(all_max_rates_per_frame) / len(all_max_rates_per_frame)\n", + " avg_max_rate_per_second = sum(all_max_rates_per_second) / len(all_max_rates_per_second)\n", + " \n", + " print(\"\\nOverall Statistics:\")\n", + " print(f\"Average maximum rate of change: {avg_max_rate_per_frame:.2f} degrees/frame\")\n", + " print(f\"Average maximum rate of change: {avg_max_rate_per_second:.2f} degrees/second\")\n", + " \n", + " if all_min_to_max_times:\n", + " avg_min_to_max_time = sum(all_min_to_max_times) / len(all_min_to_max_times)\n", + " avg_min_to_max_rate = sum(all_min_to_max_rates) / len(all_min_to_max_rates)\n", + " max_min_to_max_rate = max(abs(rate) for rate in all_min_to_max_rates)\n", + " \n", + " print(f\"Average time from min to max: {avg_min_to_max_time:.2f}s\")\n", + " print(f\"Average rate from min to max: {avg_min_to_max_rate:.2f} degrees/second\")\n", + " print(f\"Maximum rate from min to max: {max_min_to_max_rate:.2f} degrees/second\")\n", + " \n", + " # Add a summary section with all periods on one plot\n", + " plt.figure(figsize=(14, 8))\n", + " \n", + " # Plot all periods with different colors\n", + " for i, (start_idx, end_idx) in enumerate(active_periods):\n", + " period_timestamps = timestamps[start_idx:end_idx+1]\n", + " period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n", + " norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n", + " \n", + " plt.plot(norm_timestamps, period_angle_cmds, label=f\"Period {i+1}\")\n", + " \n", + " plt.title(\"LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)\")\n", + " plt.xlabel(\"Time (s)\")\n", + " plt.ylabel(\"LKAS_ANGLE_CMD (degrees)\")\n", + " plt.grid(True)\n", + " plt.legend()\n", + " \n", + " # Create a histogram of min-to-max times\n", + " if all_min_to_max_times:\n", + " plt.figure(figsize=(10, 6))\n", + " plt.hist(all_min_to_max_times, bins=15)\n", + " plt.title(\"Histogram of Min-to-Max Transition Times\")\n", + " plt.xlabel(\"Time (s)\")\n", + " plt.ylabel(\"Frequency\")\n", + " plt.grid(True)\n", + " \n", + " # Create a histogram of min-to-max rates\n", + " plt.figure(figsize=(10, 6))\n", + " plt.hist(all_min_to_max_rates, bins=15)\n", + " plt.title(\"Histogram of Min-to-Max Transition Rates\")\n", + " plt.xlabel(\"Rate (degrees/second)\")\n", + " plt.ylabel(\"Frequency\")\n", + " plt.grid(True)\n", + " \n", + " plt.tight_layout()\n", + " plt.show()\n", + " \n", + " # Also display the original figure with individual period plots\n", + " fig.tight_layout()\n", + " plt.show()\n", + "else:\n", + " print(\"No active periods found to analyze\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.12" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +}