mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

FrogPilot 0.9.7

Browse Source
This commit is contained in:
FrogAi
2024-06-27 10:22:08 -07:00
parent da00915ac8
commit b705b02e70
682 changed files with 181798 additions and 1348 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/torqued.py
+18 -6
View File
@@ -11,6 +11,8 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles
HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
MIN_POINTS_TOTAL = 4000
@@ -179,7 +181,7 @@ class TorqueEstimator(ParameterEstimator):
if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
self.filtered_points.add_point(float(steer), float(lateral_acc))
def get_msg(self, valid=True, with_points=False):
def get_msg(self, valid=True, with_points=False, frogpilot_toggles=None):
msg = messaging.new_message('liveTorqueParameters')
msg.valid = valid
liveTorqueParameters = msg.liveTorqueParameters
@@ -207,9 +209,9 @@ class TorqueEstimator(ParameterEstimator):
if with_points:
liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x if not frogpilot_toggles.use_custom_lat_accel_factor else frogpilot_toggles.steer_lat_accel_factor)
liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x if not frogpilot_toggles.use_custom_steer_friction else frogpilot_toggles.steer_friction)
liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
liveTorqueParameters.decay = self.decay
liveTorqueParameters.maxResets = self.resets
@@ -220,12 +222,18 @@ def main(demo=False):
config_realtime_process([0, 1, 2, 3], 5)
pm = messaging.PubMaster(['liveTorqueParameters'])
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman', 'frogpilotPlan'], poll='liveLocationKalman')
params = Params()
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
estimator = TorqueEstimator(CP)
# FrogPilot variables
frogpilot_toggles = get_frogpilot_toggles()
if not frogpilot_toggles.liveValid:
estimator = TorqueEstimator(CP, True)
while True:
sm.update()
if sm.all_checks():
@@ -236,13 +244,17 @@ def main(demo=False):
# 4Hz driven by liveLocationKalman
if sm.frame % 5 == 0:
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=False, frogpilot_toggles=frogpilot_toggles))
# Cache points every 60 seconds while onroad
if sm.frame % 240 == 0:
msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
msg = estimator.get_msg(valid=sm.all_checks(), with_points=True, frogpilot_toggles=frogpilot_toggles)
params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())
# Update FrogPilot parameters
if sm['frogpilotPlan'].togglesUpdated:
frogpilot_toggles = get_frogpilot_toggles()
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description='Process the --demo argument.')