Weather condition offsets

frogpilot/controls/frogpilot_planner.py

@@ -18,6 +18,7 @@ from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcc
 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
+from openpilot.frogpilot.controls.lib.weather_checker import WeatherChecker
 
 class FrogPilotPlanner:
   def __init__(self, error_log, ThemeManager):
@@ -29,6 +30,7 @@ class FrogPilotPlanner:
     self.frogpilot_events = FrogPilotEvents(self, error_log, ThemeManager)
     self.frogpilot_following = FrogPilotFollowing(self)
     self.frogpilot_vcruise = FrogPilotVCruise(self)
+    self.frogpilot_weather = WeatherChecker(self)
 
     self.driving_in_curve = False
     self.lateral_check = False
@@ -111,6 +113,11 @@ class FrogPilotPlanner:
 
     self.v_cruise = self.frogpilot_vcruise.update(long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles)
 
+    if self.gps_position and time_validated and frogpilot_toggles.weather_presets:
+      self.frogpilot_weather.update_weather(now, frogpilot_toggles)
+    else:
+      self.frogpilot_weather.weather_id = 0
+
   def update_lead_status(self):
     following_lead = self.lead_one.status
     following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE
@@ -148,6 +155,8 @@ class FrogPilotPlanner:
       frogpilotPlan.increasedStoppedDistance = 0
     else:
       frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance
+      if self.frogpilot_weather.weather_id != 0:
+        frogpilotPlan.increasedStoppedDistance += self.frogpilot_weather.increase_stopped_distance
 
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
@@ -175,4 +184,7 @@ class FrogPilotPlanner:
 
     frogpilotPlan.vCruise = float(self.v_cruise)
 
+    frogpilotPlan.weatherDaytime = self.frogpilot_weather.is_daytime
+    frogpilotPlan.weatherId = self.frogpilot_weather.weather_id
+
     pm.send("frogpilotPlan", frogpilot_plan_send)

frogpilot/controls/lib/frogpilot_acceleration.py

@@ -76,6 +76,9 @@ class FrogPilotAcceleration:
       self.max_accel = get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise)
       self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel)
 
+    if self.frogpilot_planner.frogpilot_weather.weather_id != 0:
+      self.max_accel -= self.max_accel * self.frogpilot_planner.frogpilot_weather.reduce_acceleration
+
     if self.frogpilot_planner.tracking_lead:
       self.min_accel = ACCEL_MIN
     elif sm["frogpilotCarState"].forceCoast:

frogpilot/controls/lib/frogpilot_following.py

@@ -3,7 +3,7 @@ import numpy as np
 
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
 
-from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
+from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW
 
 TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
 
@@ -65,6 +65,9 @@ class FrogPilotFollowing:
 
     self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
 
+    if self.frogpilot_planner.frogpilot_weather.weather_id != 0:
+      self.t_follow = min(self.t_follow + self.frogpilot_planner.frogpilot_weather.increase_following_distance, MAX_T_FOLLOW)
+
     if long_control_active and self.frogpilot_planner.tracking_lead:
       if not sm["frogpilotCarState"].trafficModeEnabled and frogpilot_toggles.human_following:
         self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead)

frogpilot/controls/lib/weather_checker.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+import os
+import requests
+import time
+
+from concurrent.futures import ThreadPoolExecutor
+
+from openpilot.frogpilot.common.frogpilot_utilities import calculate_distance_to_point, is_url_pingable
+
+CACHE_DISTANCE = 25
+MAX_RETRIES = 3
+RETRY_DELAY = 60
+
+# Reference: https://openweathermap.org/weather-conditions
+WEATHER_CATEGORIES = {
+  "RAIN": {
+    "ranges": [(300, 321), (500, 504)],
+    "suffix": "rain",
+  },
+  "RAIN_STORM": {
+    "ranges": [(200, 232), (511, 511), (520, 531)],
+    "suffix": "rain_storm",
+  },
+  "SNOW": {
+    "ranges": [(600, 622)],
+    "suffix": "snow",
+  },
+  "LOW_VISIBILITY": {
+    "ranges": [(701, 762)],
+    "suffix": "low_visibility",
+  },
+  "CLEAR": {
+    "ranges": [(800, 800)],
+    "suffix": "clear",
+  },
+}
+
+class WeatherChecker:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.is_daytime = False
+
+    self.api_25_calls = 0
+    self.api_3_calls = 0
+    self.increase_following_distance = 0
+    self.increase_stopped_distance = 0
+    self.reduce_acceleration = 0
+    self.reduce_lateral_acceleration = 0
+    self.sunrise = 0
+    self.sunset = 0
+    self.weather_id = 0
+
+    self.hourly_forecast = None
+    self.last_gps_position = None
+    self.last_updated = None
+
+    user_api_key = self.frogpilot_planner.params.get("WeatherToken")
+    self.api_key = user_api_key or os.environ.get("WEATHER_TOKEN", "")
+
+    if user_api_key:
+      self.check_interval = 60
+    else:
+      self.check_interval = 15 * 60
+
+    self.session = requests.Session()
+    self.session.headers.update({"Accept-Language": "en"})
+    self.session.headers.update({"User-Agent": "frogpilot-weather-checker/1.0 (https://github.com/FrogAi/FrogPilot)"})
+
+    self.executor = ThreadPoolExecutor(max_workers=1)
+
+  def update_offsets(self, frogpilot_toggles):
+    suffix = WEATHER_CATEGORIES["CLEAR"]["suffix"]
+    for category in WEATHER_CATEGORIES.values():
+      if any(start <= self.weather_id <= end for start, end in category["ranges"]):
+        suffix = category["suffix"]
+        break
+
+    if suffix != WEATHER_CATEGORIES["CLEAR"]["suffix"]:
+      self.increase_following_distance = getattr(frogpilot_toggles, f"increase_following_distance_{suffix}")
+      self.increase_stopped_distance = getattr(frogpilot_toggles, f"increase_stopped_distance_{suffix}")
+      self.reduce_acceleration = getattr(frogpilot_toggles, f"reduce_acceleration_{suffix}")
+      self.reduce_lateral_acceleration = getattr(frogpilot_toggles, f"reduce_lateral_acceleration_{suffix}")
+    else:
+      self.increase_following_distance = 0
+      self.increase_stopped_distance = 0
+      self.reduce_acceleration = 0
+      self.reduce_lateral_acceleration = 0
+
+  def update_weather(self, now, frogpilot_toggles):
+    if not self.api_key:
+      self.weather_id = 0
+      return
+
+    if self.last_gps_position and self.last_updated:
+      distance = calculate_distance_to_point(
+        self.last_gps_position["latitude"],
+        self.last_gps_position["longitude"],
+        self.frogpilot_planner.gps_position.get("latitude"),
+        self.frogpilot_planner.gps_position.get("longitude")
+      )
+      if distance / 1000 > CACHE_DISTANCE:
+        self.hourly_forecast = None
+        self.last_updated = None
+
+    if self.sunrise and self.sunset:
+      self.is_daytime = self.sunrise <= int(now.timestamp()) < self.sunset
+
+    if self.last_updated and (now - self.last_updated).total_seconds() < self.check_interval:
+      if self.hourly_forecast:
+        current_forecast = min(self.hourly_forecast, key=lambda f: abs(f["dt"] - now.timestamp()))
+        self.weather_id = current_forecast.get("weather", [{}])[0].get("id", 0)
+        self.update_offsets(frogpilot_toggles)
+      return
+
+    self.last_updated = now
+
+    def complete_request(future):
+      data = future.result()
+      if data:
+        self.hourly_forecast = data.get("hourly")
+        self.last_gps_position = self.frogpilot_planner.gps_position
+
+        if "current" in data:
+          source_data = data.get("current", {})
+          current_data = source_data
+        else:
+          source_data = data
+          current_data = source_data.get("sys", source_data)
+
+        self.sunrise = current_data.get("sunrise", 0)
+        self.sunset = current_data.get("sunset", 0)
+        self.weather_id = source_data.get("weather", [{}])[0].get("id", 0)
+
+      self.update_offsets(frogpilot_toggles)
+
+    def make_request():
+      if not is_url_pingable("https://api.openweathermap.org"):
+        return None
+
+      params = {
+        "lat": self.frogpilot_planner.gps_position["latitude"],
+        "lon": self.frogpilot_planner.gps_position["longitude"],
+        "appid": self.api_key,
+        "units": "metric",
+        "exclude": "alerts,minutely,daily",
+      }
+
+      for attempt in range(1, MAX_RETRIES + 1):
+        try:
+          self.api_3_calls += 1
+          response = self.session.get("https://api.openweathermap.org/data/3.0/onecall", params=params, timeout=10)
+
+          if response.status_code in (401, 403, 429):
+            fallback_params = params.copy()
+            fallback_params.pop("exclude", None)
+            self.api_25_calls += 1
+            fallback_response = self.session.get("https://api.openweathermap.org/data/2.5/weather", params=fallback_params, timeout=10)
+            fallback_response.raise_for_status()
+            return fallback_response.json()
+
+          response.raise_for_status()
+          return response.json()
+        except Exception:
+          if attempt < MAX_RETRIES:
+            time.sleep(RETRY_DELAY)
+          continue
+      return None
+
+    future = self.executor.submit(make_request)
+    future.add_done_callback(complete_request)

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -25,6 +25,11 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent)
   loadGif("../../frogpilot/assets/other_images/turn_icon.gif", cemTurnIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/chill_mode_icon.gif", chillModeIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/experimental_mode_icon.gif", experimentalModeIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/weather_clear_day.gif", weatherClearDay, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/weather_clear_night.gif", weatherClearNight, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/weather_low_visibility.gif", weatherLowVisibility, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/weather_rain.gif", weatherRain, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/weather_snow.gif", weatherSnow, QSize(widget_size, widget_size), this);
 
   QObject::connect(animationTimer, &QTimer::timeout, [this] {
     animationFrameIndex = (animationFrameIndex + 1) % totalFrames;
@@ -179,6 +184,8 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   speedLimitSource = frogpilotPlan.getSlcSpeedLimitSource();
   stoppingDistance = modelV2.getPosition().getX().size() > 33 - 1 ? modelV2.getPosition().getX()[33 - 1] : 0.0;
   unconfirmedSpeedLimit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
+  weatherDaytime = frogpilotPlan.getWeatherDaytime();
+  weatherId = frogpilotPlan.getWeatherId();
 
   hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
   hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
@@ -316,6 +323,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
   if ((blinkerLeft || blinkerRight) && signalStyle != "None" && (standstillDuration == 0 || signalStyle != "static")) {
     paintTurnSignals(p);
   }
+
+  if (!hideBottomIcons) {
+    paintWeather(p);
+  }
 }
 
 void FrogPilotAnnotatedCameraWidget::paintAdjacentPaths(QPainter &p) {
@@ -1125,3 +1136,39 @@ void FrogPilotAnnotatedCameraWidget::paintTurnSignals(QPainter &p) {
     p.drawPixmap(signalXPosition, signalYPosition, signalWidth, signalHeight, imgToDraw);
   }
 }
+
+void FrogPilotAnnotatedCameraWidget::paintWeather(QPainter &p) {
+  if (weatherId == 0) {
+    return;
+  }
+
+  p.save();
+
+  QPoint weatherIconPosition;
+  if (compassPosition != QPoint(0, 0)) {
+    weatherIconPosition = compassPosition;
+    weatherIconPosition.rx() += (rightHandDM ? UI_BORDER_SIZE + widget_size + UI_BORDER_SIZE : -UI_BORDER_SIZE - widget_size - UI_BORDER_SIZE);
+  } else {
+    weatherIconPosition.rx() = rightHandDM ? UI_BORDER_SIZE + widget_size / 2 : width() - UI_BORDER_SIZE - btn_size;
+    weatherIconPosition.ry() = dmIconPosition.y() - widget_size / 2;
+  }
+
+  QRect weatherRect(weatherIconPosition, QSize(widget_size, widget_size));
+
+  p.setBrush(blackColor(166));
+  p.setPen(QPen(blackColor(), 10));
+  p.drawRoundedRect(weatherRect, 24, 24);
+
+  QSharedPointer<QMovie> icon = weatherDaytime ? weatherClearDay : weatherClearNight;
+  if ((weatherId >= 200 && weatherId <= 232) || (weatherId >= 300 && weatherId <= 321) || (weatherId >= 500 && weatherId <= 531)) {
+    icon = weatherRain;
+  } else if (weatherId >= 600 && weatherId <= 622) {
+    icon = weatherSnow;
+  } else if (weatherId >= 701 && weatherId <= 762) {
+    icon = weatherLowVisibility;
+  }
+
+  p.drawPixmap(weatherRect, icon->currentPixmap());
+
+  p.restore();
+}

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -74,6 +74,7 @@ private:
   void paintStandstillTimer(QPainter &p);
   void paintStoppingPoint(QPainter &p);
   void paintTurnSignals(QPainter &p);
+  void paintWeather(QPainter &p);
   void updateSignals();
 
   bool blindspotLeft;
@@ -89,6 +90,7 @@ private:
   bool longitudinalPaused;
   bool redLight;
   bool speedLimitChanged;
+  bool weatherDaytime;
 
   int animationFrameIndex;
   int desiredFollowDistance;
@@ -98,6 +100,7 @@ private:
   int signalMovement;
   int signalWidth;
   int totalFrames;
+  int weatherId;
 
   float accelerationEgo;
   float cscSpeed;
@@ -156,6 +159,11 @@ private:
   QSharedPointer<QMovie> cemTurnIcon;
   QSharedPointer<QMovie> chillModeIcon;
   QSharedPointer<QMovie> experimentalModeIcon;
+  QSharedPointer<QMovie> weatherClearDay;
+  QSharedPointer<QMovie> weatherClearNight;
+  QSharedPointer<QMovie> weatherLowVisibility;
+  QSharedPointer<QMovie> weatherRain;
+  QSharedPointer<QMovie> weatherSnow;
 
   QString leadDistanceUnit;
   QString leadSpeedUnit;