diff --git a/frogpilot/assets/other_images/curve_speed.png b/frogpilot/assets/other_images/curve_speed.png new file mode 100644 index 000000000..6ef164408 Binary files /dev/null and b/frogpilot/assets/other_images/curve_speed.png differ diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index c2de09b44..eae3ee72f 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/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) diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index f9fca93dd..a9b4a0cc3 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/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) diff --git a/frogpilot/controls/lib/curve_speed_controller.py b/frogpilot/controls/lib/curve_speed_controller.py new file mode 100644 index 000000000..36febe46e --- /dev/null +++ b/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 diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index e2acad096..e18be27ea 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/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 diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 7f9e34929..4f11a07b3 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/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(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(); +} diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 424987a8c..f82e4efa7 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/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; diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index aeb41b6d7..5e88e4dce 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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]