From 11ce6152ddf1b1b86ce1a602b008ccb3ec591a49 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Thu, 3 Dec 2020 13:12:43 -0800
Subject: [PATCH] Fix commIssue due to frame drop and power draw (#2689)

* Don't re-init pigeon while offroad (#2687)

* don't re-init pigeon while offroad

* we don't need this

* update panda

* remove check on frame drop in modeld (#2638)

Co-authored-by: robbederks <robbe.derks@gmail.com>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
---
 panda/board/boards/uno.h           |  7 ++++++-
 selfdrive/boardd/boardd.cc         | 17 +++++++++++++----
 selfdrive/modeld/models/driving.cc |  8 ++++----
 selfdrive/modeld/models/driving.h  |  1 -
 4 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h
index 91f65a246..cc9c4c0dc 100644
--- a/panda/board/boards/uno.h
+++ b/panda/board/boards/uno.h
@@ -56,7 +56,12 @@ void uno_set_gps_load_switch(bool enabled) {
 }
 
 void uno_set_bootkick(bool enabled){
-  set_gpio_output(GPIOB, 14, !enabled);
+  if(enabled){
+    set_gpio_output(GPIOB, 14, false);
+  } else {
+    // We want the pin to be floating, not forced high!
+    set_gpio_mode(GPIOB, 14, MODE_INPUT);
+  }
 }
 
 void uno_bootkick(void) {
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 1033c0825..71e450ba4 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -474,6 +474,7 @@ void pigeon_thread() {
 
   // ubloxRaw = 8042
   PubMaster pm({"ubloxRaw"});
+  bool ignition_last = false;
 
 #ifdef QCOM2
   Pigeon * pigeon = Pigeon::connect("/dev/ttyHS0");
@@ -481,19 +482,27 @@ void pigeon_thread() {
   Pigeon * pigeon = Pigeon::connect(panda);
 #endif
 
-  pigeon->init();
-
   while (!do_exit && panda->connected) {
     std::string recv = pigeon->receive();
     if (recv.length() > 0) {
       if (recv[0] == (char)0x00){
-        LOGW("received invalid ublox message, resetting panda GPS");
-        pigeon->init();
+        if (ignition) {
+          LOGW("received invalid ublox message while onroad, resetting panda GPS");
+          pigeon->init();
+        }
       } else {
         pigeon_publish_raw(pm, recv);
       }
     }
 
+    // init pigeon on rising ignition edge
+    // since it was turned off in low power mode
+    if(ignition && !ignition_last) {
+      pigeon->init();
+    }
+
+    ignition_last = ignition;
+
     // 10ms - 100 Hz
     usleep(10*1000);
   }
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index c55af9e06..ebc85f066 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -311,7 +311,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
                      float model_execution_time) {
   // make msg
   MessageBuilder msg;
-  auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2();
+  auto framed = msg.initEvent().initModelV2();
   uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
   framed.setFrameId(vipc_frame_id);
   framed.setFrameAge(frame_age);
@@ -319,7 +319,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
   framed.setTimestampEof(timestamp_eof);
   framed.setModelExecutionTime(model_execution_time);
 
-  // plan 
+  // plan
   int plan_mhp_max_idx = 0;
   for (int i=1; i<PLAN_MHP_N; i++) {
     if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
@@ -393,7 +393,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
   MessageBuilder msg;
-  auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel();
+  auto framed = msg.initEvent().initModel();
   framed.setFrameId(vipc_frame_id);
   framed.setFrameAge(frame_age);
   framed.setFrameDropPerc(frame_drop * 100);
@@ -429,7 +429,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   auto lpath = framed.initPath();
   fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx);
-  
+
   auto left_lane = framed.initLeftLane();
   int ll_idx = 1;
   fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index 346e6e876..93177105e 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -39,7 +39,6 @@
 #define POSE_SIZE 12
 
 #define MODEL_FREQ 20
-#define MAX_FRAME_DROP 0.05
 
 struct ModelDataRaw {
     float *plan;