Curve Speed Controller

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

frogpilot/common/frogpilot_utilities.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import json
 import math
+import numpy as np
 import os
 import requests
 import subprocess
@@ -80,6 +81,20 @@ def calculate_distance_to_point(lat1, lon1, lat2, lon2):
   return EARTH_RADIUS * c
 
 
+# Credit goes to Pfeiferj!
+def calculate_road_curvature(modelData):
+  orientation_rate = np.array(modelData.orientationRate.z)
+  timebase = np.array(modelData.orientationRate.t)
+  velocity = np.array(modelData.velocity.x)
+
+  lateral_acceleration = orientation_rate * velocity
+  index = np.argmax(np.abs(lateral_acceleration))
+  predicted_lateral_acc = float(lateral_acceleration[index])
+  time_to_curve = float(timebase[index])
+
+  return float(predicted_lateral_acc / max(velocity[index], 1)**2), max(time_to_curve, 1)
+
+
 def contains_event_type(events, frogpilot_events, *event_types):
   return any(events.contains(event_type) or frogpilot_events.contains(event_type) for event_type in event_types)
 

frogpilot/controls/frogpilot_planner.py

@@ -11,7 +11,8 @@ 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.common.frogpilot_utilities import calculate_road_curvature
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
 from openpilot.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration
 from openpilot.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents
@@ -29,11 +30,16 @@ class FrogPilotPlanner:
     self.frogpilot_following = FrogPilotFollowing(self)
     self.frogpilot_vcruise = FrogPilotVCruise(self)
 
+    self.driving_in_curve = False
     self.lateral_check = False
     self.model_stopped = False
+    self.road_curvature_detected = False
     self.tracking_lead = False
 
+    self.lateral_acceleration = 0
     self.model_length = 0
+    self.road_curvature = 0
+    self.time_to_curve = 0
     self.v_cruise = 0
 
     self.gps_position = None
@@ -62,6 +68,8 @@ class FrogPilotPlanner:
       self.frogpilot_cem.experimental_mode = False
       self.frogpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
 
+    self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
+
     self.frogpilot_events.update(v_cruise, sm, frogpilot_toggles)
 
     self.frogpilot_following.update(long_control_active, v_ego, sm, frogpilot_toggles)
@@ -74,12 +82,18 @@ class FrogPilotPlanner:
     }
     self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position))
 
+    self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature
+
     self.lateral_check |= sm["carState"].standstill
 
     self.model_length = sm["modelV2"].position.x[-1]
 
     self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
 
+    self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"])
+
+    self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
+
     if not sm["carState"].standstill:
       self.tracking_lead = self.update_lead_status()
 
@@ -103,6 +117,10 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk)
     frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow)
 
+    frogpilotPlan.cscControllingSpeed = self.frogpilot_vcruise.csc_controlling_speed
+    frogpilotPlan.cscSpeed = float(self.frogpilot_vcruise.csc_target)
+    frogpilotPlan.cscTraining = self.frogpilot_vcruise.csc.enable_training
+
     frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode
 
     frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg()
@@ -116,6 +134,8 @@ class FrogPilotPlanner:
 
     frogpilotPlan.redLight = self.frogpilot_cem.stop_light_detected
 
+    frogpilotPlan.roadCurvature = self.road_curvature
+
     frogpilotPlan.vCruise = float(self.v_cruise)
 
     pm.send("frogpilotPlan", frogpilot_plan_send)

frogpilot/controls/lib/curve_speed_controller.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+import numpy as np
+
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME
+
+CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL
+MAX_CURVATURE = 0.1
+MIN_CURVATURE = 0.01
+PERCENTILE = 90
+ROUNDING_PRECISION = 3
+STEP = 0.001
+
+class CurveSpeedController:
+  def __init__(self, FrogPilotVCruise):
+    self.frogpilot_planner = FrogPilotVCruise.frogpilot_planner
+
+    self.enable_training = False
+    self.target_set = False
+
+    self.training_timer = 0
+
+    self.curvature_data = self.frogpilot_planner.params.get("CurvatureData")
+
+    self.calculate_weights()
+    self.update_lateral_acceleration()
+
+  def calculate_weights(self):
+    curvatures = np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)
+    mid_point = (MIN_CURVATURE + MAX_CURVATURE) / 2
+
+    self.curvature_weights = {}
+    for curvature in curvatures:
+      distance = abs(curvature - mid_point) / (MAX_CURVATURE - MIN_CURVATURE)
+      weight = 1.0 + (4.0 * (1 - distance))
+      self.curvature_weights[str(round(curvature, ROUNDING_PRECISION))] = weight
+
+  def log_data(self, long_control_active, v_ego, sm):
+    self.enable_training = v_ego > CRUISING_SPEED
+    self.enable_training &= not self.frogpilot_planner.tracking_lead
+    self.enable_training &= not long_control_active
+
+    if self.enable_training:
+      self.training_timer += DT_MDL
+
+      if self.training_timer >= PLANNER_TIME and self.frogpilot_planner.driving_in_curve and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker):
+        lateral_acceleration = abs(self.frogpilot_planner.lateral_acceleration)
+        road_curvature = abs(round(self.frogpilot_planner.road_curvature, ROUNDING_PRECISION))
+
+        key = str(road_curvature)
+        if key in self.curvature_data:
+          data = self.curvature_data[key]
+
+          average = data["average"]
+          count = data["count"]
+
+          self.curvature_data[key] = {
+            "average": ((average * count) + lateral_acceleration) / (count + 1),
+            "count": count + 1
+          }
+        else:
+          self.curvature_data[key] = {
+            "average": lateral_acceleration,
+            "count": 1
+          }
+      else:
+        self.enable_training = False
+
+    elif self.training_timer >= PLANNER_TIME:
+      progress = 0.0
+      total_weight = 0.0
+
+      for key in list(self.curvature_weights.keys()):
+        if key in self.curvature_data:
+          progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) * self.curvature_weights[key]
+
+        total_weight += self.curvature_weights[key]
+
+      self.frogpilot_planner.params.put_nonblocking("CalibrationProgress", float(min((progress / total_weight) * 100, 100.0)))
+      self.frogpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
+      self.update_lateral_acceleration()
+
+      self.training_timer = 0
+
+    else:
+      self.training_timer = 0
+
+  def update_lateral_acceleration(self):
+    if self.curvature_data:
+      all_samples = [data["average"] for data in self.curvature_data.values()]
+      self.lateral_acceleration = float(np.percentile(all_samples, PERCENTILE))
+    else:
+      self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION
+
+    self.frogpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
+
+  def update_target(self, v_ego):
+    lateral_acceleration = self.lateral_acceleration
+    if self.frogpilot_planner.frogpilot_weather.weather_id != 0:
+      lateral_acceleration -= self.lateral_acceleration * self.frogpilot_planner.frogpilot_weather.reduce_lateral_acceleration
+
+    if self.target_set:
+      csc_speed = (lateral_acceleration / abs(self.frogpilot_planner.road_curvature))**0.5
+      decel_rate = (v_ego - csc_speed) / self.frogpilot_planner.time_to_curve
+
+      self.target -= decel_rate * DT_MDL
+      self.target = np.clip(self.target, CRUISING_SPEED, csc_speed)
+    else:
+      self.target_set = True
+      self.target = v_ego

frogpilot/controls/lib/frogpilot_vcruise.py

@@ -2,11 +2,14 @@
 from openpilot.common.constants import CV
 
 from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED
+from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController
 
 class FrogPilotVCruise:
   def __init__(self, FrogPilotPlanner):
     self.frogpilot_planner = FrogPilotPlanner
 
+    self.csc = CurveSpeedController(self)
+
   def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles):
     v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise)
     v_cruise_diff = v_cruise_cluster - v_cruise
@@ -14,7 +17,22 @@ class FrogPilotVCruise:
     v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
     v_ego_diff = v_ego_cluster - v_ego
 
-    targets = [v_cruise]
+    # FrogsGoMoo's Curve Speed Controller
+    if long_control_active and v_ego > CRUISING_SPEED and self.frogpilot_planner.road_curvature_detected and frogpilot_toggles.curve_speed_controller:
+      self.csc.update_target(v_ego)
+
+      self.csc_controlling_speed = True
+
+      self.csc_target = self.csc.target
+    else:
+      self.csc.log_data(long_control_active, v_ego, sm)
+
+      self.csc_controlling_speed = False
+      self.csc.target_set = False
+
+      self.csc_target = v_cruise
+
+    targets = [self.csc_target, v_cruise]
     v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
 
     return v_cruise

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -3,6 +3,9 @@
 FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) : QWidget(parent) {
   QSize iconSize(img_size / 4, img_size / 4);
 
+  curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size});
+  curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1));
+
   loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/lead_icon.gif", cemLeadIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/speed_icon.gif", cemSpeedIcon, QSize(widget_size, widget_size), this);
@@ -48,10 +51,22 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
 
   blindspotLeft = carState.getLeftBlindspot();
   blindspotRight = carState.getRightBlindspot();
+  cscControllingSpeed = frogpilotPlan.getCscControllingSpeed();
+  cscSpeed = frogpilotPlan.getCscSpeed();
+  cscTraining = frogpilotPlan.getCscTraining();
   experimentalMode = selfdriveState.getExperimentalMode();
+  roadCurvature = frogpilotPlan.getRoadCurvature();
 
   hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
   hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
+
+  if (cscTraining) {
+    if (!glowTimer.isValid()) {
+      glowTimer.start();
+    }
+  } else {
+    glowTimer.invalidate();
+  }
 }
 
 void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) {
@@ -72,6 +87,14 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     compassPosition.setX(0);
     compassPosition.setY(0);
   }
+
+  if (!(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) {
+    if (cscTraining) {
+      paintCurveSpeedControlTraining(p);
+    } else if (isCruiseSet && cscControllingSpeed) {
+      paintCurveSpeedControl(p);
+    }
+  }
 }
 
 void FrogPilotAnnotatedCameraWidget::paintBlindSpotPath(QPainter &p) {
@@ -225,3 +248,61 @@ void FrogPilotAnnotatedCameraWidget::paintCompass(QPainter &p) {
 
   p.restore();
 }
+
+void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControl(QPainter &p) {
+  p.save();
+
+  QRect curveSpeedRect(QPoint(setSpeedRect.right() + UI_BORDER_SIZE, setSpeedRect.top()), QSize(defaultSize.width() * 1.25, defaultSize.width() * 1.25));
+
+  QPixmap &curveSpeedImage = roadCurvature < 0 ? curveSpeedIcon : curveSpeedIconFlipped;
+  QSize curveSpeedSize = curveSpeedImage.size();
+  QPoint curveSpeedPoint = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignCenter, curveSpeedSize, curveSpeedRect).topLeft();
+
+  p.setOpacity(1.0);
+
+  QRect cscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 100));
+  p.setBrush(blueColor(166));
+  p.setFont(InterFont(45, QFont::Bold));
+  p.setPen(QPen(blueColor(), 10));
+  p.drawRoundedRect(cscRect, 24, 24);
+  p.setPen(QPen(whiteColor(), 6));
+  p.drawText(cscRect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, QString::number(std::nearbyint(fmin(speed, cscSpeed * speedConversion))) + speedUnit);
+  p.drawPixmap(curveSpeedPoint, curveSpeedImage);
+
+  p.restore();
+}
+
+void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p) {
+  p.save();
+
+  qreal phase = (glowTimer.elapsed() % 2000) / 2000.0 * 2 * M_PI;
+  qreal alphaFactor = 0.5 + 0.5 * sin(phase);
+
+  QColor glowColor = blueColor();
+  glowColor.setAlphaF(0.3 + 0.7 * alphaFactor);
+
+  int glowWidth = 8 + static_cast<int>(2 * alphaFactor);
+
+  QRect curveSpeedRect(QPoint(setSpeedRect.right() + UI_BORDER_SIZE, setSpeedRect.top()), QSize(defaultSize.width() * 1.25, defaultSize.width() * 1.25));
+
+  QPixmap &curveSpeedImage = roadCurvature < 0 ? curveSpeedIcon : curveSpeedIconFlipped;
+  QSize curveSpeedSize = curveSpeedImage.size();
+  QPoint curveSpeedPoint = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignCenter, curveSpeedSize, curveSpeedRect).topLeft();
+
+  p.setOpacity(1.0);
+
+  p.setBrush(blackColor(166));
+  p.setPen(QPen(glowColor, glowWidth));
+  p.drawRoundedRect(curveSpeedRect, 24, 24);
+  p.drawPixmap(curveSpeedPoint, curveSpeedImage);
+  p.setBrush(blackColor(166));
+  p.setFont(InterFont(35, QFont::Bold));
+  p.setPen(QPen(blackColor(), 10));
+
+  QRect textRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 50));
+  p.drawRoundedRect(textRect, 24, 24);
+  p.setPen(QPen(whiteColor(), 6));
+  p.drawText(textRect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, "Training...");
+
+  p.restore();
+}

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -43,12 +43,18 @@ protected:
 private:
   void paintCEMStatus(QPainter &p);
   void paintCompass(QPainter &p);
+  void paintCurveSpeedControl(QPainter &p);
+  void paintCurveSpeedControlTraining(QPainter &p);
 
   bool blindspotLeft;
   bool blindspotRight;
+  bool cscControllingSpeed;
+  bool cscTraining;
   bool experimentalMode;
 
+  float cscSpeed;
   float distanceConversion;
+  float roadCurvature;
   float setSpeed;
   float speedConversion;
   float speedConversionMetrics;
@@ -59,6 +65,11 @@ private:
   QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); }
   QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
 
+  QElapsedTimer glowTimer;
+
+  QPixmap curveSpeedIcon;
+  QPixmap curveSpeedIconFlipped;
+
   QPoint cemStatusPosition;
   QPoint compassPosition;
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -129,7 +129,8 @@ class LongitudinalPlanner:
     if mode == 'acc':
       accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
-      accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+      if not sm['frogpilotPlan'].cscControllingSpeed:
+        accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
     else:
       accel_clip = [ACCEL_MIN, ACCEL_MAX]