dragonpilot beta3

date: 2024-07-24T16:04:16
commit: dc9f089

CHANGELOGS.md

@@ -2,13 +2,63 @@ dragonpilot beta3 0.9.8
 =======================
 * Up to comma.ai openpilot master branch commit cbee4421da690fa7b260f62c4a2c72ccacc6c902 (2024-07-14)
 * DP HIGHLIGHT:
-  * Fixed Lexus IS/GS crashed issue.
+  * [BUGFIX] Fixed Lexus IS/GS crashed issue.
+  * [BUGFIX] [TESTING] Fixed issue with TeToo road match. 
+  * [NEW] [TESTING] Personalized Accel Learner (PAL) - Learn and apply accel habits from driver.
+    * Ability to set a launch boost for initial acceleration  
+    * Ability to freeze / reset learned parameters
+  * [NEW] Delete logs button (Device Panel)
+  * [NEW] [VAG] Patch for Modified PQ Platform
+  * [NEW] Lane Priority Mode
+    * Ability to use Lane Planner for lane centering instead of end to end (when lane probs are high, logic by sunnyhaibin).
+    * Ability to set speed limit and only activate at high speed (e.g. highway use only)
+
+
+===============
+DP FEATURE LIST
+===============
+  * Common / Lateral
+    * Always on Lane Keeping Assist (ALKA)
+    * Configurable Lane Change Assist w/ Auto Lane Change Support
+    * Road Edge Detection. (by rav4kumar)
+  * Common / Longitudinal
+    * Dynamic End-to-End Controller
+    * On-Screen Driving Personality Button
+    * Speed-based Alternative Driving Personality Mode
+  * Common / UI
+    * Display Flight Panel
+    * Display Rainbow Path
+    * Accel/Decel Indicator
+    * Blinker / Brake Indicator
+  * Common / Tē-Tôo (Map)
+    * [TESTING] Display Road Name (Global, Online OSM)
+    * [TESTING] Display Speed Camera Warning (Global, Online OSM)
+    * [TESTING] Display Speed Camera Warning for Taiwan (Offline DB)
+  * Common / Device
+    * Disable Logging
+    * Disable Upload While On-Road (by rav4kumar)
+    * Display On/Off Mode
+    * Audible Alert Mode
+    * Auto Shutdown w/ Timer
+    * On-Road / Off-Road Switcher (Software Panel)
+    * Debug Console
+  * Vehicle / Toyota
+    * Stop and Go Mod
+    * Enhanced BSM (by rav4kumar)
+    * Auto Door Lock/Unlock
+    * Zorro-Steering-Sensor (ZSS) Support (by erichMoraga)
+    * PCM Compensation Enhancement (by cydia2020)
+    * Auto Brake-Hold (by alexandreSato)
+  * Vehicle / VAG
+    * SnG for A0 Platform (by Saber422)
+  * Vehicle / HKG
+    * Low Speed Turn Enhancement
 
 dragonpilot beta3 [2024.07.13]
 =======================
 * Up to comma.ai openpilot master branch commit 01c2174d5968266b87f1d1fecefce5affaeaa624 (2024-07-02)
 * DP HIGHLIGHT:
-  * (TESTING) Tē-Tôo / Map Module
+  * [NEW] [TESTING] Tē-Tôo / Map Module
     * Road Name Display (Online using OSM)
     * Speed Camera Warning (Online using OSM: Untested)
     * Speed Camera Warning (Taiwan, Offline)

body/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[8] = "26f4dbe6";
+const uint8_t gitversion[8] = "3552e110";

cereal/gen/cpp/log.capnp.c++

@@ -28658,7 +28658,7 @@ static const ::capnp::_::AlignedData<2198> b_d314cfd957229c11 = {
      82,   0, 147, 255,   0,   0,   0,   0,
       0,   0,   1,   0, 110,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,
-     21,  19,   0,   0, 130,   0,   0,   0,
+     21,  19,   0,   0,  98,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,
      20,  19,   0,   0,   3,   0,   1,   0,
      32,  19,   0,   0,   2,   0,   1,   0,
@@ -29880,10 +29880,10 @@ static const ::capnp::_::AlignedData<2198> b_d314cfd957229c11 = {
      16,   0,   0,   0,   0,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,
-     99, 117, 115, 116, 111, 109,  82, 101,
-    115, 101, 114, 118, 101, 100,  51,   0,
+    108,  97, 116, 101, 114,  97, 108,  80,
+    108,  97, 110,   0,   0,   0,   0,   0,
      16,   0,   0,   0,   0,   0,   0,   0,
-     53,  76,  68, 131, 152,  87, 150, 218,
+     30, 181, 232,  42, 142,  49, 233, 225,
       0,   0,   0,   0,   0,   0,   0,   0,
       0,   0,   0,   0,   0,   0,   0,   0,
      16,   0,   0,   0,   0,   0,   0,   0,
@@ -30130,7 +30130,6 @@ static const ::capnp::_::RawSchema* const d_d314cfd957229c11[] = {
   &s_d7700859ed1f5b76,
   &s_d817d6655115ca85,
   &s_d9058dcb967c2753,
-  &s_da96579883444c35,
   &s_db98be6565516acb,
   &s_dc24138990726023,
   &s_de266b39b76b461e,
@@ -30160,11 +30159,11 @@ static const ::capnp::_::RawSchema* const d_d314cfd957229c11[] = {
   &s_fe346a9de48d9b50,
   &s_fe35ad896ffaeacf,
 };
-static const uint16_t m_d314cfd957229c11[] = {98, 101, 30, 20, 55, 42, 60, 63, 5, 23, 127, 69, 22, 28, 35, 7, 107, 110, 111, 112, 113, 114, 115, 116, 124, 125, 126, 6, 70, 87, 76, 71, 59, 92, 128, 85, 26, 10, 91, 21, 48, 3, 41, 40, 99, 100, 1, 65, 64, 32, 96, 19, 8, 46, 25, 72, 51, 44, 37, 62, 36, 61, 94, 16, 14, 122, 119, 120, 117, 121, 118, 49, 18, 0, 24, 108, 95, 78, 105, 103, 9, 75, 82, 104, 83, 38, 84, 27, 68, 54, 58, 56, 47, 53, 45, 12, 81, 80, 33, 89, 90, 31, 13, 2, 86, 15, 17, 4, 11, 73, 109, 97, 123, 52, 66, 43, 34, 39, 102, 57, 50, 106, 79, 93, 67, 74, 88, 77, 29};
+static const uint16_t m_d314cfd957229c11[] = {98, 101, 30, 20, 55, 42, 60, 63, 5, 23, 127, 69, 22, 28, 35, 7, 107, 111, 112, 113, 114, 115, 116, 124, 125, 126, 6, 70, 87, 76, 71, 59, 92, 128, 85, 26, 10, 91, 21, 48, 3, 41, 40, 99, 100, 1, 65, 110, 64, 32, 96, 19, 8, 46, 25, 72, 51, 44, 37, 62, 36, 61, 94, 16, 14, 122, 119, 120, 117, 121, 118, 49, 18, 0, 24, 108, 95, 78, 105, 103, 9, 75, 82, 104, 83, 38, 84, 27, 68, 54, 58, 56, 47, 53, 45, 12, 81, 80, 33, 89, 90, 31, 13, 2, 86, 15, 17, 4, 11, 73, 109, 97, 123, 52, 66, 43, 34, 39, 102, 57, 50, 106, 79, 93, 67, 74, 88, 77, 29};
 static const uint16_t i_d314cfd957229c11[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 0, 67};
 const ::capnp::_::RawSchema s_d314cfd957229c11 = {
   0xd314cfd957229c11, b_d314cfd957229c11.words, 2198, d_d314cfd957229c11, m_d314cfd957229c11,
-  88, 129, i_d314cfd957229c11, nullptr, nullptr, { &s_d314cfd957229c11, nullptr, nullptr, 0, 0, nullptr }, true
+  87, 129, i_d314cfd957229c11, nullptr, nullptr, { &s_d314cfd957229c11, nullptr, nullptr, 0, 0, nullptr }, true
 };
 #endif  // !CAPNP_LITE
 }  // namespace schemas

cereal/gen/cpp/log.capnp.h

@@ -2545,7 +2545,7 @@ struct Event {
     CONTROLS_STATE_EXT,
     LONGITUDINAL_PLAN_EXT,
     TE_TOO,
-    CUSTOM_RESERVED3,
+    LATERAL_PLAN,
     CUSTOM_RESERVED4,
     CUSTOM_RESERVED5,
     CUSTOM_RESERVED6,
@@ -19688,9 +19688,9 @@ class Event::Reader {
   inline bool hasTeToo() const;
   inline  ::cereal::TeToo::Reader getTeToo() const;
 
-  inline bool isCustomReserved3() const;
-  inline bool hasCustomReserved3() const;
-  inline  ::cereal::CustomReserved3::Reader getCustomReserved3() const;
+  inline bool isLateralPlan() const;
+  inline bool hasLateralPlan() const;
+  inline  ::cereal::LateralPlan::Reader getLateralPlan() const;
 
   inline bool isCustomReserved4() const;
   inline bool hasCustomReserved4() const;
@@ -20663,13 +20663,13 @@ class Event::Builder {
   inline void adoptTeToo(::capnp::Orphan< ::cereal::TeToo>&& value);
   inline ::capnp::Orphan< ::cereal::TeToo> disownTeToo();
 
-  inline bool isCustomReserved3();
-  inline bool hasCustomReserved3();
-  inline  ::cereal::CustomReserved3::Builder getCustomReserved3();
-  inline void setCustomReserved3( ::cereal::CustomReserved3::Reader value);
-  inline  ::cereal::CustomReserved3::Builder initCustomReserved3();
-  inline void adoptCustomReserved3(::capnp::Orphan< ::cereal::CustomReserved3>&& value);
-  inline ::capnp::Orphan< ::cereal::CustomReserved3> disownCustomReserved3();
+  inline bool isLateralPlan();
+  inline bool hasLateralPlan();
+  inline  ::cereal::LateralPlan::Builder getLateralPlan();
+  inline void setLateralPlan( ::cereal::LateralPlan::Reader value);
+  inline  ::cereal::LateralPlan::Builder initLateralPlan();
+  inline void adoptLateralPlan(::capnp::Orphan< ::cereal::LateralPlan>&& value);
+  inline ::capnp::Orphan< ::cereal::LateralPlan> disownLateralPlan();
 
   inline bool isCustomReserved4();
   inline bool hasCustomReserved4();
@@ -53284,57 +53284,57 @@ inline ::capnp::Orphan< ::cereal::TeToo> Event::Builder::disownTeToo() {
       ::capnp::bounded<0>() * ::capnp::POINTERS));
 }
 
-inline bool Event::Reader::isCustomReserved3() const {
-  return which() == Event::CUSTOM_RESERVED3;
+inline bool Event::Reader::isLateralPlan() const {
+  return which() == Event::LATERAL_PLAN;
 }
-inline bool Event::Builder::isCustomReserved3() {
-  return which() == Event::CUSTOM_RESERVED3;
+inline bool Event::Builder::isLateralPlan() {
+  return which() == Event::LATERAL_PLAN;
 }
-inline bool Event::Reader::hasCustomReserved3() const {
-  if (which() != Event::CUSTOM_RESERVED3) return false;
+inline bool Event::Reader::hasLateralPlan() const {
+  if (which() != Event::LATERAL_PLAN) return false;
   return !_reader.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
 }
-inline bool Event::Builder::hasCustomReserved3() {
-  if (which() != Event::CUSTOM_RESERVED3) return false;
+inline bool Event::Builder::hasLateralPlan() {
+  if (which() != Event::LATERAL_PLAN) return false;
   return !_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
 }
-inline  ::cereal::CustomReserved3::Reader Event::Reader::getCustomReserved3() const {
-  KJ_IREQUIRE((which() == Event::CUSTOM_RESERVED3),
+inline  ::cereal::LateralPlan::Reader Event::Reader::getLateralPlan() const {
+  KJ_IREQUIRE((which() == Event::LATERAL_PLAN),
               "Must check which() before get()ing a union member.");
-  return ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::get(_reader.getPointerField(
+  return ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::get(_reader.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS));
 }
-inline  ::cereal::CustomReserved3::Builder Event::Builder::getCustomReserved3() {
-  KJ_IREQUIRE((which() == Event::CUSTOM_RESERVED3),
+inline  ::cereal::LateralPlan::Builder Event::Builder::getLateralPlan() {
+  KJ_IREQUIRE((which() == Event::LATERAL_PLAN),
               "Must check which() before get()ing a union member.");
-  return ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::get(_builder.getPointerField(
+  return ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::get(_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS));
 }
-inline void Event::Builder::setCustomReserved3( ::cereal::CustomReserved3::Reader value) {
+inline void Event::Builder::setLateralPlan( ::cereal::LateralPlan::Reader value) {
   _builder.setDataField<Event::Which>(
-      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::CUSTOM_RESERVED3);
-  ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::set(_builder.getPointerField(
+      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::LATERAL_PLAN);
+  ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::set(_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS), value);
 }
-inline  ::cereal::CustomReserved3::Builder Event::Builder::initCustomReserved3() {
+inline  ::cereal::LateralPlan::Builder Event::Builder::initLateralPlan() {
   _builder.setDataField<Event::Which>(
-      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::CUSTOM_RESERVED3);
-  return ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::init(_builder.getPointerField(
+      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::LATERAL_PLAN);
+  return ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::init(_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS));
 }
-inline void Event::Builder::adoptCustomReserved3(
-    ::capnp::Orphan< ::cereal::CustomReserved3>&& value) {
+inline void Event::Builder::adoptLateralPlan(
+    ::capnp::Orphan< ::cereal::LateralPlan>&& value) {
   _builder.setDataField<Event::Which>(
-      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::CUSTOM_RESERVED3);
-  ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::adopt(_builder.getPointerField(
+      ::capnp::bounded<4>() * ::capnp::ELEMENTS, Event::LATERAL_PLAN);
+  ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::adopt(_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS), kj::mv(value));
 }
-inline ::capnp::Orphan< ::cereal::CustomReserved3> Event::Builder::disownCustomReserved3() {
-  KJ_IREQUIRE((which() == Event::CUSTOM_RESERVED3),
+inline ::capnp::Orphan< ::cereal::LateralPlan> Event::Builder::disownLateralPlan() {
+  KJ_IREQUIRE((which() == Event::LATERAL_PLAN),
               "Must check which() before get()ing a union member.");
-  return ::capnp::_::PointerHelpers< ::cereal::CustomReserved3>::disown(_builder.getPointerField(
+  return ::capnp::_::PointerHelpers< ::cereal::LateralPlan>::disown(_builder.getPointerField(
       ::capnp::bounded<0>() * ::capnp::POINTERS));
 }
 

cereal/log.capnp

@@ -2352,7 +2352,7 @@ struct Event {
     controlsStateExt @107 :Custom.ControlsStateExt;
     longitudinalPlanExt @108 :Custom.LongitudinalPlanExt;
     teToo @109 :Custom.TeToo;
-    customReserved3 @110 :Custom.CustomReserved3;
+    lateralPlan @110 :LateralPlan;
     customReserved4 @111 :Custom.CustomReserved4;
     customReserved5 @112 :Custom.CustomReserved5;
     customReserved6 @113 :Custom.CustomReserved6;

cereal/services.h

@@ -81,6 +81,7 @@ static std::map<std::string, service> services = {
   { "controlsStateExt", {"controlsStateExt", false, 100, 10}},
   { "longitudinalPlanExt", {"longitudinalPlanExt", false, 20, 5}},
   { "teToo", {"teToo", false, 5, -1}},
+  { "lateralPlan", {"lateralPlan", false, 20, 5}},
 };
 #endif
 

cereal/services.py

@@ -92,6 +92,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
   "controlsStateExt": (False, 100., 10),
   "longitudinalPlanExt": (False, 20., 5),
   "teToo": (False, 5),
+  "lateralPlan": (False, 20., 5),
 }
 SERVICE_LIST = {name: Service(*vals) for
                 idx, (name, vals) in enumerate(_services.items())}

dp_ext/selfdrive/controls/latpland.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+from cereal import car
+from openpilot.common.params import Params
+from openpilot.common.realtime import Priority, config_realtime_process
+from openpilot.dp_ext.selfdrive.controls.lib.lateral_mpc_lane_planner import LateralMpcLanePlanner
+import cereal.messaging as messaging
+
+def latpland_thread():
+    config_realtime_process(5, Priority.CTRL_LOW)
+
+    params = Params()
+    with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
+        CP = msg
+
+    lmlp = LateralMpcLanePlanner(CP)
+
+    pm = messaging.PubMaster(['lateralPlan'])
+    sm = messaging.SubMaster(['carState', 'modelV2'], poll='modelV2')
+
+    while True:
+        sm.update()
+
+        if sm.updated['modelV2']:
+            plan_send = messaging.new_message('lateralPlan')
+            plan_send.valid = sm.all_checks(service_list=['carState', 'modelV2'])
+            lat_plan = plan_send.lateralPlan
+
+            lmlp.update_md(sm['modelV2'])
+            lmlp.update_v_ego(sm['carState'].vEgo)
+
+            if lmlp.should_activate():
+                lmlp.update()
+
+                lat_plan.psis = lmlp.get_psis()
+                lat_plan.curvatures = lmlp.get_curvatures()
+                # lat_plan.curvatureRates = lmlp.get_curvature_rates()
+
+            lat_plan.useLaneLines = lmlp.is_active()
+
+            pm.send('lateralPlan', plan_send)
+
+
+def main():
+    latpland_thread()
+
+if __name__ == "__main__":
+    main()

dp_ext/selfdrive/controls/lib/lane_planner.py

@@ -1,15 +1,12 @@
 '''
-This is the lane_planner from 0.8.16
+This is the lane_planner from 0.8.17 with slight modification to support new lateral_mpc_lane_planner
+https://raw.githubusercontent.com/commaai/openpilot/01a73b14d8b23c17bc6c7b314a266d594e88eb55/selfdrive/controls/lib/lane_planner.py
 
-reason I keep this as a separate file is that Nuclear Grade model released during 0.8.15 / 0.8.16.
-So it could handle better with old planners.
-
-Note 1: This may not work in newer version.
+Note: This may not work in the future.
 
 '''
 
 import numpy as np
-from cereal import log
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.numpy_fast import interp
 from openpilot.common.realtime import DT_MDL
@@ -19,19 +16,18 @@
 TRAJECTORY_SIZE = 33
 # camera offset is meters from center car to camera
 # model path is in the frame of the camera
-
+PATH_OFFSET = 0.00
 CAMERA_OFFSET = 0.04
-PATH_OFFSET = 0.04
 
 class LanePlanner:
   def __init__(self):
     self.ll_t = np.zeros((TRAJECTORY_SIZE,))
     self.ll_x = np.zeros((TRAJECTORY_SIZE,))
     self.lll_y = np.zeros((TRAJECTORY_SIZE,))
     self.rll_y = np.zeros((TRAJECTORY_SIZE,))
-    self.lane_width_estimate = FirstOrderFilter(2.7, 9.95, DT_MDL)
+    self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
     self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
-    self.lane_width = 2.7
+    self.lane_width = 3.7
 
     self.lll_prob = 0.
     self.rll_prob = 0.
@@ -40,12 +36,12 @@ def __init__(self):
     self.lll_std = 0.
     self.rll_std = 0.
 
-    self.l_lane_change_prob = 0.
-    self.r_lane_change_prob = 0.
-
     self.camera_offset = CAMERA_OFFSET
     self.path_offset = PATH_OFFSET
 
+  def update_offsets(self, camera_offset):
+    self.camera_offset = camera_offset
+
   def parse_model(self, md):
     lane_lines = md.laneLines
     if len(lane_lines) == 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
@@ -59,11 +55,6 @@ def parse_model(self, md):
       self.lll_std = md.laneLineStds[1]
       self.rll_std = md.laneLineStds[2]
 
-    desire_state = md.meta.desireState
-    if len(desire_state):
-      self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
-      self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
-
   def get_d_path(self, v_ego, path_t, path_xyz):
     # Reduce reliance on lanelines that are too far apart or
     # will be in a few seconds
@@ -88,7 +79,7 @@ def get_d_path(self, v_ego, path_t, path_xyz):
     self.lane_width_certainty.update(l_prob * r_prob)
     current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
     self.lane_width_estimate.update(current_lane_width)
-    speed_lane_width = interp(v_ego, [0., 31.], [2.7, 3.5])
+    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
     self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
                       (1 - self.lane_width_certainty.x) * speed_lane_width