Tē-Tôo module

cereal/custom.capnp

@@ -26,7 +26,33 @@ enum LaneChangeAssistMode {
   auto @3;
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct TeToo @0xf35cc4560bbf6ec2 {
+  lat @0 :Float32;
+  lon @1 :Float32;
+  bearing @2 :Float32;
+  name @3 :Text;
+  maxspeed @4 :Float32;
+  tags @5 :Text;
+
+  updatingData @6 :Bool;
+  nearestFeatures @7 :List(Feature);
+
+  struct Feature {
+    id @0 :Text;
+    type @1 :FeatureType;
+    lat @2 :Float32;
+    lon @3 :Float32;
+    bearing @4 :Float32;
+    distance @5 :Float32;
+    tags @6 :Text;
+    probability @7 :Float32;
+  }
+
+  enum FeatureType {
+    trafficSignal @0;
+    speedCamera @1;
+  }
+
 }
 
 struct CustomReserved3 @0xda96579883444c35 {

cereal/log.capnp

@@ -2351,7 +2351,7 @@ struct Event {
     # *********** Custom: reserved for forks ***********
     controlsStateExt @107 :Custom.ControlsStateExt;
     longitudinalPlanExt @108 :Custom.LongitudinalPlanExt;
-    customReserved2 @109 :Custom.CustomReserved2;
+    teToo @109 :Custom.TeToo;
     customReserved3 @110 :Custom.CustomReserved3;
     customReserved4 @111 :Custom.CustomReserved4;
     customReserved5 @112 :Custom.CustomReserved5;

cereal/services.py

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

common/params.cc

@@ -237,6 +237,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_toyota_auto_brake_hold", PERSISTENT},
     {"dp_device_offroad", CLEAR_ON_MANAGER_START},
     {"dp_toyota_sng", PERSISTENT},
+    {"dp_tetoo", PERSISTENT},
+    {"dp_tetoo_data", PERSISTENT},
+    {"dp_tetoo_gps", PERSISTENT},
+    {"dp_tetoo_taiwan_speed_camera", PERSISTENT},
 };
 
 } // namespace

launch_chffrplus.sh

@@ -76,6 +76,15 @@ function launch {
   # hardware specific init
   if [ -f /AGNOS ]; then
     agnos_init
+    # dp
+    if ! (LD_LIBRARY_PATH=$DIR/third_party/libspatialindex/larch64/lib/ python -c "import rtree") &> /dev/null; then
+      pip install "$DIR/dp_ext/selfdrive/tetood/pkgs/Rtree-1.2.0-cp311-cp311-linux_aarch64.whl"
+    fi
+  else
+    # dp
+    if ! (LD_LIBRARY_PATH=$DIR/third_party/libspatialindex/x86_64/lib/ python -c "import rtree") &> /dev/null; then
+      pip install "$DIR/dp_ext/selfdrive/tetood/pkgs/Rtree-1.2.0-cp312-cp312-linux_x86_64.whl"
+    fi
   fi
 
   # write tmux scrollback to a file

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -47,6 +47,7 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
   rainbow_path = new RainbowPath;
   flight_panel = new FlightPanel;
   chevron_ext = new ChevronExt;
+  tt_indicator = new TeTooIndicator;
   #endif
 
 }
@@ -94,6 +95,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   rainbow_path->update_states(s);
   flight_panel->update_states(s, is_metric);
   chevron_ext->update_states(s, is_metric);
+  tt_indicator->update_states(s);
   #endif
 }
 
@@ -145,6 +147,11 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   p.setFont(InterFont(66));
   drawText(p, rect().center().x(), 290, speedUnit, 200);
 
+  #ifdef DP
+  tt_indicator->paint_speed_camera(p, rect(), speedStr);
+  tt_indicator->paint_maxspeed(p, set_speed_rect.topRight());
+  #endif
+
   p.restore();
 }
 
@@ -395,6 +402,7 @@ void AnnotatedCameraWidget::paintGL() {
     update_model(s, model);
 
     #ifdef DP
+    tt_indicator->paint_road_name(painter, rect().left(), rect().bottom(), rect().width());
     flight_panel->paint(painter, width(), height());
     #endif
 

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -44,7 +44,8 @@ class AnnotatedCameraWidget : public CameraWidget {
   PersonalityButton *personality_btn;
   KnightScanner *knight_scanner;
   RainbowPath *rainbow_path;
-  ChevronExt * chevron_ext;
+  ChevronExt *chevron_ext;
+  TeTooIndicator *tt_indicator;
   #endif
 
 protected:

selfdrive/ui/translations/main_zh-CHT.ts

@@ -224,7 +224,7 @@ Your vehicle must support openpilot longitudinal control.</source>
 Once activate, the lead vehicle indicator will display an &quot;A&quot;</source>
         <translation>當速度低於設定值時，openpilot將會切換到指定的「駕駛風格」。
 1 km/h = 0.62 mph。
-一旦啟用，前車指示將顯示一個「A」。</translation>
+一旦啟用，前車指示將顯示一個「A」</translation>
     </message>
     <message>
         <source>OFF</source>
@@ -525,6 +525,40 @@ Reboot Required.</source>
 感謝 Alexandre Sato 的貢獻：https://github.com/AlexandreSato/
 需要重新啟動。</translation>
     </message>
+    <message>
+        <source>Enable Stop and Go (SnG) Hack</source>
+        <translation>啟用 Stop and Go (SnG) 修正</translation>
+    </message>
+    <message>
+        <source>When enabled, openpilot will stop sending standstill signal when the car is fully stopped.
+ONLY WORK ON SOME VEHICLES.
+Reboot Required.</source>
+        <translation>啟用後，openpilot 在車輛完全停止時將不再發送停止信號。
+僅適用於部分車輛。
+需要重新啟動。</translation>
+    </message>
+    <message>
+        <source>Display Road Name</source>
+        <translation>顯示道路名稱</translation>
+    </message>
+    <message>
+        <source>When enabled, openpilot will display live road name using OpenStreetMap data.
+Internet required.</source>
+        <translation>啟用後，openpilot 將使用 OpenStreetMap 資料顯示即時道路名稱。
+需要網路連線。</translation>
+    </message>
+    <message>
+        <source>Display Speed Camera Warning (Taiwan)</source>
+        <translation>顯示測速照相警告（台灣）</translation>
+    </message>
+    <message>
+        <source>Tē-Tôo (Map)</source>
+        <translation>地圖</translation>
+    </message>
+    <message>
+        <source>When enabled, openpilot will display speed camera warning using offline database.</source>
+        <translation>啟用後，openpilot 將使用離線資料庫顯示測速照相警告。</translation>
+    </message>
 </context>
 <context>
     <name>DeclinePage</name>
@@ -1322,6 +1356,26 @@ This may take up to a minute.</source>
         <source>never</source>
         <translation>從未更新</translation>
     </message>
+    <message>
+        <source>Onroad/Offroad Mode</source>
+        <translation>上線/離線 模式</translation>
+    </message>
+    <message>
+        <source>Go Offroad</source>
+        <translation>切換到離線</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to switch driving mode?</source>
+        <translation>您確定要切換模式嗎？</translation>
+    </message>
+    <message>
+        <source>CONFIRM</source>
+        <translation>確認</translation>
+    </message>
+    <message>
+        <source>Go Onroad</source>
+        <translation>切換到上線</translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>

selfdrive/ui/ui.cc

@@ -252,6 +252,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "carControl", "controlsStateExt",
     // de2e
     "longitudinalPlanExt",
+    // TeToo
+    "teToo",
   });
 
   Params params;

system/manager/manager.py

@@ -75,6 +75,10 @@ def manager_init() -> None:
     ("dp_toyota_enhanced_bsm", "0"),
     ("dp_toyota_auto_brake_hold", "0"),
     ("dp_toyota_sng", "0"),
+    ("dp_tetoo", "0"),
+    ("dp_tetoo_data", ""),
+    ("dp_tetoo_gps", ""),
+    ("dp_tetoo_taiwan_speed_camera", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))

system/manager/process_config.py

@@ -53,6 +53,9 @@ def dp_onroad_uploads(started, params, CP: car.CarParams) -> bool:
 def dpdmonitoringd(started, params, CP: car.CarParams) -> bool:
   return params.get_bool("dp_device_dm_unavailable") and started
 
+def tetood(started, params, CP: car.CarParams) -> bool:
+  return started and (params.get_bool("dp_tetoo") or params.get_bool("dp_tetoo_taiwan_speed_camera"))
+
 procs = [
   DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"),
 
@@ -100,6 +103,7 @@ def dpdmonitoringd(started, params, CP: car.CarParams) -> bool:
 
   #dp
   PythonProcess("dpdmonitoringd", "dp_ext.selfdrive.monitoring.dmonitoringd", dpdmonitoringd, enabled=not PC),
+  NativeProcess("tetood", "dp_ext/selfdrive/tetood", ["./tetood"], tetood),
 ]
 
 managed_processes = {p.name: p for p in procs}

third_party/libspatialindex/larch64/include/spatialindex/Ball.h

@@ -0,0 +1,98 @@
+/******************************************************************************
+ * Project:  libspatialindex - A C++ library for spatial indexing
+ * Author:   Peter Labadorf - [email protected]
+ ******************************************************************************
+ * Copyright (c) 2023, Peter Labadorf
+ *
+ * All rights reserved.
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+******************************************************************************/
+
+#pragma once
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace SpatialIndex
+{
+
+	class SIDX_DLL Ball: public Tools::IObject, public virtual IShape
+	{
+	public:
+		Ball();
+		Ball(double radius, const Point& center);
+		Ball(double radius, const double *pCoords, uint32_t dimension);
+		Ball(const Ball& b);
+		~Ball() override;
+
+		virtual Ball& operator=(const Ball& b);
+		virtual bool operator==(const Ball& b) const;
+
+		//
+		// IObject interface
+		//
+
+		Ball* clone() override;
+
+		//
+		// ISerializable interface
+		//
+
+		uint32_t getByteArraySize() override;
+		void loadFromByteArray(const uint8_t *data) override;
+		void storeToByteArray(uint8_t **data, uint32_t &length) override;
+
+		//
+		// IShape interface
+		//
+
+		bool intersectsShape(const IShape &in) const override;
+		bool containsShape(const IShape &in) const override;
+		bool touchesShape(const IShape &in) const override;
+		void getCenter(SpatialIndex::Point &out) const override;
+		uint32_t getDimension() const override;
+		void getMBR(SpatialIndex::Region &out) const override;
+		double getArea() const override;
+		double getMinimumDistance(const IShape &in) const override;
+
+		virtual bool containsLineSegment(const SpatialIndex::LineSegment *line) const;
+		virtual bool containsRegion(const SpatialIndex::Region *region) const;
+
+		inline bool containsPoint(const Point *point) const
+		{
+			return getMinimumDistance(*point) <= m_centerPoint.m_dimension;
+		}
+
+		inline bool containsBall(const Ball *ball) const
+		{
+			return getMinimumDistance(ball->m_centerPoint) + ball->m_radius <= m_radius;
+		}
+
+	public:
+		double m_radius{0.0};
+		Point m_centerPoint;
+
+	}; // Ball
+
+	SIDX_DLL std::ostream& operator<<(std::ostream& os, const Ball& ball);
+
+}
+