mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-03 06:33:50 +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/paramsd.py
+19 -3
View File
@@ -14,6 +14,7 @@ from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKi
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, params_memory
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
@@ -96,8 +97,9 @@ class ParamsLearner:
self.steering_angle = msg.steeringAngleDeg
self.speed = msg.vEgo
complex_dynamics = abs(msg.aEgo) > 1.0 or abs(msg.steeringRateDeg) > 20
in_linear_region = abs(self.steering_angle) < 45
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region and not complex_dynamics
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
@@ -124,7 +126,7 @@ def main():
REPLAY = bool(int(os.getenv("REPLAY", "0")))
pm = messaging.PubMaster(['liveParameters'])
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman')
sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'frogpilotPlan'], poll='liveLocationKalman')
params_reader = Params()
# wait for stats about the car to come in from controls
@@ -182,6 +184,9 @@ def main():
total_offset_valid = True
roll_valid = True
# FrogPilot variables
frogpilot_toggles = get_frogpilot_toggles()
while True:
sm.update()
if sm.all_checks():
@@ -191,6 +196,12 @@ def main():
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
location = sm['liveLocationKalman']
if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
bearing = math.degrees(location.calibratedOrientationNED.value[2])
lat = location.positionGeodetic.value[0]
lon = location.positionGeodetic.value[1]
params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)):
@@ -219,7 +230,7 @@ def main():
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
liveParameters.steerRatio = float(x[States.STEER_RATIO].item() if not frogpilot_toggles.use_custom_steer_ratio else frogpilot_toggles.steer_ratio)
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
liveParameters.roll = roll
liveParameters.angleOffsetAverageDeg = angle_offset_average
@@ -232,6 +243,8 @@ def main():
0.2 <= liveParameters.stiffnessFactor <= 5.0,
min_sr <= liveParameters.steerRatio <= max_sr,
))
if CP.carFingerprint == "RAM_HD" or CP.carName == "subaru" and CP.lateralTuning.which() == "torque":
liveParameters.valid = True
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
@@ -255,6 +268,9 @@ def main():
pm.send('liveParameters', msg)
# Update FrogPilot parameters
if sm['frogpilotPlan'].togglesUpdated:
frogpilot_toggles = get_frogpilot_toggles()
if __name__ == "__main__":
main()