From 8ab6ec02adee3fb9957c38aab04cfe1ba1e942c3 Mon Sep 17 00:00:00 2001
From: ZwX1616 <zwx1616@gmail.com>
Date: Wed, 9 Dec 2020 20:05:47 -0800
Subject: [PATCH] road camera malfunction alert (#2697)

* camera unresponsive

* blue and remove loge

* simplify

* should override to avoid blank screen

* let it be

* stop sound

Co-authored-by: WXEon <device@comma.ai>
old-commit-hash: 257c5f9d946b1392ce205140b4bb24843968976e
---
 selfdrive/ui/paint.hpp |  2 +-
 selfdrive/ui/ui.cc     | 20 +++++++++++++++++---
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.hpp
index e71a2a43ec..afebd64d25 100644
--- a/selfdrive/ui/paint.hpp
+++ b/selfdrive/ui/paint.hpp
@@ -2,7 +2,7 @@
 #include "ui.hpp"
 
 
-void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, int va_color,
+void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, UIStatus va_color,
                           const char* va_text1, const char* va_text2);
 void ui_draw(UIState *s);
 void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha);
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 8a0ceded00..ce672672d1 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -25,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param
 }
 
 void ui_init(UIState *s) {
-  s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
+  s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame",
                          "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"});
 
   s->started = false;
@@ -262,7 +262,7 @@ void ui_update(UIState *s) {
     s->scene.alert_size = cereal::ControlsState::AlertSize::NONE;
   }
 
-  // Handle controls timeout
+  // Handle controls/fcamera timeout
   if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 5*UI_FREQ) {
     if ((s->sm)->rcv_frame("controlsState") < s->started_frame) {
       // car is started, but controlsState hasn't been seen at all
@@ -271,7 +271,8 @@ void ui_update(UIState *s) {
       s->scene.alert_size = cereal::ControlsState::AlertSize::MID;
     } else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 5*UI_FREQ) {
       // car is started, but controls is lagging or died
-      if (s->scene.alert_text2 != "Controls Unresponsive") {
+      if (s->scene.alert_text2 != "Controls Unresponsive" &&
+          s->scene.alert_text1 != "Camera Malfunction") {
         s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT);
         LOGE("Controls unresponsive");
       }
@@ -281,6 +282,19 @@ void ui_update(UIState *s) {
       s->scene.alert_size = cereal::ControlsState::AlertSize::FULL;
       s->status = STATUS_ALERT;
     }
+
+    if (((s->sm)->rcv_frame("frame") > s->started_frame &&
+                ((s->sm)->frame - (s->sm)->rcv_frame("frame")) > 5*UI_FREQ) ||
+               ((s->sm)->frame - (s->sm)->rcv_frame("frame")) > 15*UI_FREQ) {
+      // controls is fine, but rear camera is lagging or died
+      s->scene.alert_text1 = "Camera Malfunction";
+      s->scene.alert_text2 = "Contact Support";
+      s->scene.alert_size = cereal::ControlsState::AlertSize::FULL;
+      s->status = STATUS_DISENGAGED;
+      s->sound->stop();
+      ui_draw_vision_alert(s, s->scene.alert_size, s->status,
+                           s->scene.alert_text1.c_str(), s->scene.alert_text2.c_str());
+    }
   }
 
   // Read params