brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-05-05 05:10:06 -05:00
parent 38fb8ffde0
commit f015a8368b
11 changed files with 89 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
selfdrive/frogpilot/controls/lib/frogpilot_functions.py
View File

@@ -31,15 +31,14 @@ def calculate_lane_width(lane, current_lane, road_edge):
return min(distance_to_lane, distance_to_road_edge)
# Clearpilot added default value 0
def calculate_road_curvature(modelData, v_ego):
predicted_velocities = np.array(modelData.velocity.x)
# clearpilot fix
# /data/openpilot/openpilot/selfdrive/frogpilot/controls/lib/frogpilot_functions.py:36: RuntimeWarning: divide by zero encountered in divide
# curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2)
curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2)
# /data/openpilot/openpilot/selfdrive/frogpilot/controls/lib/frogpilot_functions.py:37: RuntimeWarning: invalid value encountered in multiply
# return np.amax(curvature_ratios * (v_ego**2))
return np.amax(curvature_ratios * (v_ego**2))
try:
predicted_velocities = np.array(modelData.velocity.x)
curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2)
return np.amax(curvature_ratios * (v_ego**2))
except:
return 0
class MovingAverageCalculator:
def __init__(self):
@@ -141,6 +140,10 @@ class FrogPilotFunctions:
except OSError:
pass
@classmethod
def prune_recordings():
return
@classmethod
def setup_frogpilot(cls):
remount_cmd = ['sudo', 'mount', '-o', 'remount,rw', '/persist']