FrogPilot structs

frogpilot/controls/frogpilot_card.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.cruise import ButtonType
+
+class FrogPilotCard:
+  def __init__(self, CP, FPCP):
+    self.CP = CP
+
+    self.params = Params(return_defaults=True)
+    self.params_memory = Params(memory=True)
+
+    self.accel_pressed = False
+    self.decel_pressed = False
+
+  def update(self, carState, frogpilotCarState, sm):
+    if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents):
+      self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents)
+
+    if sm.updated["frogpilotPlan"] or any(be.type == ButtonType.decelCruise for be in carState.buttonEvents):
+      self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents)
+
+    frogpilotCarState.accelPressed = self.accel_pressed
+    frogpilotCarState.decelPressed = self.decel_pressed
+
+    return frogpilotCarState

frogpilot/controls/frogpilot_planner.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+import json
+
+import cereal.messaging as messaging
+
+from openpilot.common.constants import CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
+
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, THRESHOLD
+from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration
+from openpilot.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents
+from openpilot.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing
+from openpilot.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise
+
+class FrogPilotPlanner:
+  def __init__(self):
+    self.params = Params(return_defaults=True)
+    self.params_memory = Params(memory=True)
+
+    self.frogpilot_acceleration = FrogPilotAcceleration(self)
+    self.frogpilot_events = FrogPilotEvents(self)
+    self.frogpilot_following = FrogPilotFollowing(self)
+    self.frogpilot_vcruise = FrogPilotVCruise(self)
+
+    self.lateral_check = False
+    self.model_stopped = False
+    self.tracking_lead = False
+
+    self.model_length = 0
+    self.v_cruise = 0
+
+    self.gps_position = None
+
+    self.gps_location_service = get_gps_location_service(self.params)
+
+    self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL)
+
+  def update(self, now, time_validated, sm):
+    self.lead_one = sm["radarState"].leadOne
+
+    long_control_active = sm["carControl"].longActive
+
+    v_cruise = min(sm["carState"].vCruise, V_CRUISE_MAX) * CV.KPH_TO_MS
+    v_ego = max(sm["carState"].vEgo, 0)
+
+    if long_control_active:
+      self.frogpilot_acceleration.update(v_ego, sm)
+    else:
+      self.frogpilot_acceleration.max_accel = 0
+      self.frogpilot_acceleration.min_accel = 0
+
+    self.frogpilot_events.update(v_cruise, sm)
+
+    self.frogpilot_following.update(long_control_active, v_ego, sm)
+
+    gps_location = sm[self.gps_location_service]
+    self.gps_position = {
+      "latitude": gps_location.latitude,
+      "longitude": gps_location.longitude,
+      "bearing": gps_location.bearingDeg,
+    }
+    self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position))
+
+    self.lateral_check |= sm["carState"].standstill
+
+    self.model_length = sm["modelV2"].position.x[-1]
+
+    self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+
+    if not sm["carState"].standstill:
+      self.tracking_lead = self.update_lead_status()
+
+    self.v_cruise = self.frogpilot_vcruise.update(long_control_active, now, time_validated, v_cruise, v_ego, sm)
+
+  def update_lead_status(self):
+    following_lead = self.lead_one.status
+    following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE
+
+    self.tracking_lead_filter.update(following_lead)
+    return self.tracking_lead_filter.x >= THRESHOLD
+
+  def publish(self, sm, pm):
+    frogpilot_plan_send = messaging.new_message("frogpilotPlan")
+    frogpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])
+    frogpilotPlan = frogpilot_plan_send.frogpilotPlan
+
+    frogpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.frogpilot_following.acceleration_jerk)
+    frogpilotPlan.dangerFactor = float(self.frogpilot_following.danger_factor)
+    frogpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.frogpilot_following.danger_jerk)
+    frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk)
+    frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow)
+
+    frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg()
+
+    frogpilotPlan.lateralCheck = self.lateral_check
+
+    frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel)
+    frogpilotPlan.minAcceleration = float(self.frogpilot_acceleration.min_accel)
+
+    frogpilotPlan.vCruise = float(self.v_cruise)
+
+    pm.send("frogpilotPlan", frogpilot_plan_send)

frogpilot/controls/lib/frogpilot_acceleration.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel
+
+class FrogPilotAcceleration:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.max_accel = 0
+    self.min_accel = 0
+
+  def update(self, v_ego, sm, frogpilot_toggles):
+    self.max_accel = get_max_accel(v_ego)
+
+    if self.frogpilot_planner.tracking_lead:
+      self.min_accel = ACCEL_MIN
+    else:
+      self.min_accel = ACCEL_MIN

frogpilot/controls/lib/frogpilot_events.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+from openpilot.selfdrive.selfdrived.events import ET, EVENT_NAME, FROGPILOT_EVENT_NAME, EventName, FrogPilotEventName, Events
+
+class FrogPilotEvents:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.events = Events(frogpilot=True)
+
+    self.startup_seen = False
+
+    self.played_events = set()
+
+  def update(self, v_cruise, sm):
+    current_alert = sm["selfdriveState"].alertType
+    current_frogpilot_alert = sm["selfdriveState"].alertType
+
+    alerts_empty = all(sm[state].alertText1 == "" and sm[state].alertText2 == "" for state in ["selfdriveState", "frogpilotSelfdriveState"])
+
+    self.events.clear()
+
+    self.startup_seen |= sm["frogpilotSelfdriveState"].alertText1 == frogpilot_toggles.startup_alert_top and sm["frogpilotSelfdriveState"].alertText2 == frogpilot_toggles.startup_alert_bottom
+
+    self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names)

frogpilot/controls/lib/frogpilot_following.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_jerk_factor, get_T_FOLLOW
+
+class FrogPilotFollowing:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.following_lead = False
+
+    self.acceleration_jerk = 0
+    self.danger_jerk = 0
+    self.speed_jerk = 0
+    self.t_follow = 0
+
+  def update(self, long_control_active, v_ego, sm):
+    if long_control_active:
+      if sm["carState"].aEgo >= 0:
+        self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
+          frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed,
+          frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed,
+          frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed,
+          frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality
+        )
+      else:
+        self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
+          frogpilot_toggles.aggressive_jerk_deceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed_decrease,
+          frogpilot_toggles.standard_jerk_deceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed_decrease,
+          frogpilot_toggles.relaxed_jerk_deceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed_decrease,
+          frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality
+        )
+
+      self.t_follow = get_T_FOLLOW(
+        frogpilot_toggles.aggressive_follow,
+        frogpilot_toggles.standard_follow,
+        frogpilot_toggles.relaxed_follow,
+        frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality
+      )
+    else:
+      self.base_acceleration_jerk = 0
+      self.base_danger_jerk = 0
+      self.base_speed_jerk = 0
+      self.t_follow = 0
+
+    self.acceleration_jerk = self.base_acceleration_jerk
+    self.danger_jerk = self.base_danger_jerk
+    self.speed_jerk = self.base_speed_jerk
+
+    self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
+
+    if long_control_active and self.frogpilot_planner.tracking_lead:
+      self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead)
+
+  def update_follow_values(self, lead_distance, v_ego, v_lead):

frogpilot/controls/lib/frogpilot_vcruise.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+from openpilot.common.constants import CV
+
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED
+
+class FrogPilotVCruise:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+  def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm):
+    v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise)
+    v_cruise_diff = v_cruise_cluster - v_cruise
+
+    v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
+    v_ego_diff = v_ego_cluster - v_ego
+
+    targets = [v_cruise]
+    v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
+
+    return v_cruise

frogpilot/frogpilot_process.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import datetime
+import json
 import time
 
 from cereal import messaging
@@ -7,11 +8,14 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, Ratekeeper, config_realtime_process
 from openpilot.common.time_helpers import system_time_valid
 
+from openpilot.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
+
 ASSET_CHECK_RATE = (1 / DT_MDL)
 
 def check_assets(params_memory):
 
-def transition_offroad(time_validated, sm, params):
+def transition_offroad(frogpilot_planner, time_validated, sm, params):
+  params.put("LastGPSPosition", json.dumps(frogpilot_planner.gps_position))
 
 def transition_onroad():
 
@@ -26,9 +30,11 @@ def frogpilot_thread():
 
   config_realtime_process(5, Priority.CTRL_LOW)
 
+  pm = messaging.PubMaster(["frogpilotPlan"])
   sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState",
                             "gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2",
-                            "onroadEvents", "pandaStates", "radarState", "selfdriveState"],
+                            "onroadEvents", "pandaStates", "radarState", "selfdriveState", "frogpilotCarState",
+                            "frogpilotSelfdriveState", "frogpilotModelV2", "frogpilotOnroadEvents"],
                             poll="modelV2")
 
   params = Params(return_defaults=True)
@@ -46,14 +52,20 @@ def frogpilot_thread():
     started = sm["deviceState"].started
 
     if not started and started_previously:
-      transition_offroad(time_validated, sm, params)
+      transition_offroad(frogpilot_planner, time_validated, sm, params)
 
       run_update_checks = True
     elif started and not started_previously:
+      frogpilot_planner = FrogPilotPlanner()
+
       transition_onroad()
 
     if started and sm.updated["modelV2"]:
+      frogpilot_planner.update(now, time_validated, sm)
+      frogpilot_planner.publish(sm, pm)
     elif not started:
+      frogpilot_plan_send = messaging.new_message("frogpilotPlan")
+      pm.send("frogpilotPlan", frogpilot_plan_send)
 
     started_previously = started
 

frogpilot/ui/frogpilot_ui.cc

@@ -10,6 +10,12 @@ static void update_state(FrogPilotUIState *fs) {
     const cereal::DeviceState::Reader &deviceState = fpsm["deviceState"].getDeviceState();
     frogpilot_scene.online = deviceState.getNetworkType() != cereal::DeviceState::NetworkType::NONE;
   }
+  if (fpsm.updated("frogpilotCarState")) {
+    const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState();
+  }
+  if (fpsm.updated("frogpilotPlan")) {
+    const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan();
+  }
   if (fpsm.updated("selfdriveState")) {
     const cereal::SelfdriveState::Reader &selfdriveState = fpsm["selfdriveState"].getSelfdriveState();
     frogpilot_scene.enabled = selfdriveState.getEnabled();
@@ -18,8 +24,8 @@ static void update_state(FrogPilotUIState *fs) {
 
 FrogPilotUIState::FrogPilotUIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
-    "carControl", "deviceState",
-    "liveDelay",
+    "carControl", "deviceState", "frogpilotCarState", "frogpilotDeviceState",
+    "frogpilotPlan", "frogpilotRadarState", "frogpilotSelfdriveState", "liveDelay",
     "liveParameters", "liveTorqueParameters", "liveTracks", "selfdriveState"
   });
 

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -14,6 +14,9 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   const SubMaster &fpsm = *(fs.sm);
 
   const cereal::CarState::Reader &carState = sm["carState"].getCarState();
+  const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState();
+  const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan();
+  const cereal::FrogPilotSelfdriveState::Reader &frogpilotSelfdriveState = fpsm["frogpilotSelfdriveState"].getFrogpilotSelfdriveState();
   const cereal::ModelDataV2::Reader &modelV2 = sm["modelV2"].getModelV2();
   const cereal::SelfdriveState::Reader &selfdriveState = sm["selfdriveState"].getSelfdriveState();
 
@@ -36,6 +39,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   }
 
   hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
+  hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
 }
 
 void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) {