Squash merge of 'commaai/master'. The only effect on the subaru-

specific code is to change the return from CarController.upate
to (actuators, can_sends) instead of just (can_sends).

RELEASES.md

@@ -1,3 +1,6 @@
+Version 0.8.13 (2022-XX-XX)
+========================
+
 Version 0.8.12 (2021-12-15)
 ========================
  * New driving model

common/ffi_wrapper.py

@@ -28,7 +28,7 @@ def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries=
     try:
       mod = __import__(cache)
     except Exception:
-      print("cache miss {0}".format(cache))
+      print(f"cache miss {cache}")
       compile_code(cache, c_code, c_header, tmpdir, cflags, libraries)
       mod = __import__(cache)
   finally:

common/file_helpers.py

@@ -35,7 +35,7 @@ def get_tmpdir_on_same_filesystem(path):
   if len(parts) > 1 and parts[1] == "scratch":
     return "/scratch/tmp"
   elif len(parts) > 2 and parts[2] == "runner":
-    return "/{}/runner/tmp".format(parts[1])
+    return f"/{parts[1]}/runner/tmp"
   return "/tmp"
 
 

common/profiler.py

@@ -42,4 +42,4 @@ def display(self):
         print("%30s: %9.2f  avg: %7.2f  percent: %3.0f   IGNORED" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100))
       else:
         print("%30s: %9.2f  avg: %7.2f  percent: %3.0f" % (n, ms*1000.0, ms*1000.0/self.iter, ms/self.tot*100))
-    print("Iter clock: %2.6f   TOTAL: %2.2f" % (self.tot/self.iter, self.tot))
+    print(f"Iter clock: {self.tot / self.iter:2.6f}   TOTAL: {self.tot:2.2f}")

common/realtime.py

@@ -79,7 +79,7 @@ def monitor_time(self) -> bool:
     remaining = self._next_frame_time - sec_since_boot()
     self._next_frame_time += self._interval
     if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
-      print("%s lagging by %.2f ms" % (self._process_name, -remaining * 1000))
+      print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
       lagged = True
     self._frame += 1
     self._remaining = remaining

common/tests/test_file_helpers.py

@@ -8,7 +8,7 @@
 
 class TestFileHelpers(unittest.TestCase):
   def run_atomic_write_func(self, atomic_write_func):
-    path = "/tmp/tmp{}".format(uuid4())
+    path = f"/tmp/tmp{uuid4()}"
     with atomic_write_func(path) as f:
       f.write("test")
 

common/timeout.py

@@ -12,7 +12,7 @@ class Timeout:
   """
   def __init__(self, seconds, error_msg=None):
     if error_msg is None:
-      error_msg = 'Timed out after {} seconds'.format(seconds)
+      error_msg = f'Timed out after {seconds} seconds'
     self.seconds = seconds
     self.error_msg = error_msg
 

selfdrive/athena/athenad.py

@@ -56,6 +56,28 @@
 cur_upload_items = {}
 
 
+class UploadQueueCache():
+  params = Params()
+
+  @staticmethod
+  def initialize(upload_queue):
+    try:
+      upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue")
+      if upload_queue_json is not None:
+        for item in json.loads(upload_queue_json):
+          upload_queue.put(UploadItem(**item))
+    except Exception:
+      cloudlog.exception("athena.UploadQueueCache.initialize.exception")
+
+  @staticmethod
+  def cache(upload_queue):
+    try:
+      items = [i._asdict() for i in upload_queue.queue if i.id not in cancelled_uploads]
+      UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items))
+    except Exception:
+      cloudlog.exception("athena.UploadQueueCache.cache.exception")
+
+
 def handle_long_poll(ws):
   end_event = threading.Event()
 
@@ -111,6 +133,7 @@ def upload_handler(end_event):
 
     try:
       cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True)
+
       if cur_upload_items[tid].id in cancelled_uploads:
         cancelled_uploads.remove(cur_upload_items[tid].id)
         continue
@@ -120,6 +143,7 @@ def cb(sz, cur):
           cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
 
         _do_upload(cur_upload_items[tid], cb)
+        UploadQueueCache.cache(upload_queue)
       except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
         cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
 
@@ -131,6 +155,8 @@ def cb(sz, cur):
             current=False
           )
           upload_queue.put_nowait(item)
+          UploadQueueCache.cache(upload_queue)
+
           cur_upload_items[tid] = None
 
           for _ in range(RETRY_DELAY):
@@ -248,6 +274,7 @@ def uploadFileToUrl(fn, url, headers):
   item = item._replace(id=upload_id)
 
   upload_queue.put_nowait(item)
+  UploadQueueCache.cache(upload_queue)
 
   return {"enqueued": 1, "item": item._asdict()}
 
@@ -280,8 +307,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
 
     cloudlog.debug("athena.startLocalProxy.starting")
 
-    params = Params()
-    dongle_id = params.get("DongleId").decode('utf8')
+    dongle_id = Params().get("DongleId").decode('utf8')
     identity_token = Api(dongle_id).get_token()
     ws = create_connection(remote_ws_uri,
                            cookie="jwt=" + identity_token,
@@ -525,6 +551,7 @@ def backoff(retries):
 def main():
   params = Params()
   dongle_id = params.get("DongleId", encoding='utf-8')
+  UploadQueueCache.initialize(upload_queue)
 
   ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
   api = Api(dongle_id)

selfdrive/athena/manage_athenad.py

@@ -6,20 +6,20 @@
 from common.params import Params
 from selfdrive.manager.process import launcher
 from selfdrive.swaglog import cloudlog
-from selfdrive.version import get_version, get_dirty
+from selfdrive.version import get_version, is_dirty
 
 ATHENA_MGR_PID_PARAM = "AthenadPid"
 
 
 def main():
   params = Params()
   dongle_id = params.get("DongleId").decode('utf-8')
-  cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty())
+  cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty())
 
   try:
     while 1:
       cloudlog.info("starting athena daemon")
-      proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad',))
+      proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad'))
       proc.start()
       proc.join()
       cloudlog.event("athenad exited", exitcode=proc.exitcode)

selfdrive/athena/tests/helpers.py

@@ -50,18 +50,28 @@ def get_token(self):
 
 
 class MockParams():
-  def __init__(self):
-    self.params = {
-      "DongleId": b"0000000000000000",
-      "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private"  # noqa: E501
-    }
+  default_params = {
+    "DongleId": b"0000000000000000",
+    "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private",  # noqa: E501
+    "AthenadUploadQueue": '[]'
+  }
+  params = default_params.copy()
+
+  @staticmethod
+  def restore_defaults():
+    MockParams.params = MockParams.default_params.copy()
 
   def get(self, k, encoding=None):
-    ret = self.params.get(k)
+    ret = MockParams.params.get(k)
     if ret is not None and encoding is not None:
       ret = ret.decode(encoding)
     return ret
 
+  def put(self, k, v):
+    if k not in MockParams.params:
+      raise KeyError(f"key: {k} not in MockParams")
+    MockParams.params[k] = v
+
 
 class MockWebsocket():
   def __init__(self, recv_queue, send_queue):

selfdrive/athena/tests/test_athenad.py

@@ -21,19 +21,22 @@
 from selfdrive.athena.tests.helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server
 from cereal import messaging
 
+
 class TestAthenadMethods(unittest.TestCase):
   @classmethod
   def setUpClass(cls):
     cls.SOCKET_PORT = 45454
+    athenad.Params = MockParams
     athenad.ROOT = tempfile.mkdtemp()
     athenad.SWAGLOG_DIR = swaglog.SWAGLOG_DIR = tempfile.mkdtemp()
-    athenad.Params = MockParams
     athenad.Api = MockApi
     athenad.LOCAL_PORT_WHITELIST = set([cls.SOCKET_PORT])
 
   def setUp(self):
+    MockParams.restore_defaults()
     athenad.upload_queue = queue.Queue()
     athenad.cur_upload_items.clear()
+    athenad.cancelled_uploads.clear()
 
     for i in os.listdir(athenad.ROOT):
       p = os.path.join(athenad.ROOT, i)
@@ -249,6 +252,26 @@ def test_listUploadQueue(self):
     items = dispatcher["listUploadQueue"]()
     self.assertEqual(len(items), 0)
 
+  def test_upload_queue_persistence(self):
+    item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1')
+    item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2')
+
+    athenad.upload_queue.put_nowait(item1)
+    athenad.upload_queue.put_nowait(item2)
+
+    # Ensure cancelled items are not persisted
+    athenad.cancelled_uploads.add(item2.id)
+
+    # serialize item
+    athenad.UploadQueueCache.cache(athenad.upload_queue)
+
+    # deserialize item
+    athenad.upload_queue.queue.clear()
+    athenad.UploadQueueCache.initialize(athenad.upload_queue)
+
+    self.assertEqual(athenad.upload_queue.qsize(), 1)
+    self.assertDictEqual(athenad.upload_queue.queue[-1]._asdict(), item1._asdict())
+
   @mock.patch('selfdrive.athena.athenad.create_connection')
   def test_startLocalProxy(self, mock_create_connection):
     end_event = threading.Event()

selfdrive/boardd/boardd.cc

@@ -411,6 +411,8 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
   util::set_thread_name("boardd_panda_state");
 
   Params params;
+  SubMaster sm({"controlsState"});
+
   Panda *peripheral_panda = pandas[0];
   bool ignition_last = false;
   std::future<bool> safety_future;
@@ -445,8 +447,11 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
 
     ignition_last = ignition;
 
+    sm.update(0);
+    const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
+
     for (const auto &panda : pandas) {
-      panda->send_heartbeat();
+      panda->send_heartbeat(engaged);
     }
     util::sleep_for(500);
   }

selfdrive/boardd/panda.cc

@@ -340,8 +340,8 @@ void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode)
   usb_write(0xe6, (uint16_t)power_mode, 0);
 }
 
-void Panda::send_heartbeat() {
-  usb_write(0xf3, 1, 0);
+void Panda::send_heartbeat(bool engaged) {
+  usb_write(0xf3, engaged, 0);
 }
 
 void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {

selfdrive/boardd/panda.h

@@ -108,7 +108,7 @@ class Panda {
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
-  void send_heartbeat();
+  void send_heartbeat(bool engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);

selfdrive/camerad/cameras/camera_common.cc

@@ -200,7 +200,6 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
   framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
   framed.setTargetGreyFraction(frame_data.target_grey_fraction);
   framed.setLensPos(frame_data.lens_pos);
-  framed.setLensSag(frame_data.lens_sag);
   framed.setLensErr(frame_data.lens_err);
   framed.setLensTruePos(frame_data.lens_true_pos);
 }

selfdrive/camerad/cameras/camera_common.h

@@ -68,7 +68,6 @@ typedef struct FrameMetadata {
 
   // Focus
   unsigned int lens_pos;
-  float lens_sag;
   float lens_err;
   float lens_true_pos;
 } FrameMetadata;

selfdrive/camerad/cameras/camera_qcom.cc

@@ -852,45 +852,18 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
   s->focus_err = max_focus*1.0;
 }
 
-static std::optional<float> get_accel_z(SubMaster *sm) {
-  sm->update(0);
-  if(sm->updated("sensorEvents")) {
-    for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
-      if (event.which() == cereal::SensorEventData::ACCELERATION) {
-        if (auto v = event.getAcceleration().getV(); v.size() >= 3)
-          return -v[2];
-        break;
-      }
-    }
-  }
-  return std::nullopt;
-}
-
-static void do_autofocus(CameraState *s, SubMaster *sm) {
+static void do_autofocus(CameraState *s) {
   float lens_true_pos = s->lens_true_pos.load();
   if (!isnan(s->focus_err)) {
     // learn lens_true_pos
     const float focus_kp = 0.005;
     lens_true_pos -= s->focus_err*focus_kp;
   }
 
-  if (auto accel_z = get_accel_z(sm)) {
-    s->last_sag_acc_z = *accel_z;
-  }
-  const float sag = (s->last_sag_acc_z / 9.8) * 128;
   // stay off the walls
   lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
-  int target = std::clamp(lens_true_pos - sag, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
   s->lens_true_pos.store(lens_true_pos);
-
-  /*char debug[4096];
-  char *pdebug = debug;
-  pdebug += sprintf(pdebug, "focus ");
-  for (int i = 0; i < NUM_FOCUS; i++) pdebug += sprintf(pdebug, "%2x(%4d) ", s->confidence[i], s->focus[i]);
-  pdebug += sprintf(pdebug, "  err: %7.2f  offset: %6.2f sag: %6.2f lens_true_pos: %6.2f  cur_lens_pos: %4d->%4d", err * focus_kp, offset, sag, s->lens_true_pos, s->cur_lens_pos, target);
-  LOGD(debug);*/
-
-  actuator_move(s, target);
+  actuator_move(s, lens_true_pos);
 }
 
 void camera_autoexposure(CameraState *s, float grey_frac) {
@@ -1046,12 +1019,11 @@ static void ops_thread(MultiCameraState *s) {
   CameraExpInfo driver_cam_op;
 
   util::set_thread_name("camera_settings");
-  SubMaster sm({"sensorEvents"});
   while(!do_exit) {
     road_cam_op = road_cam_exp.load();
     if (road_cam_op.op_id != last_road_cam_op_id) {
       do_autoexposure(&s->road_cam, road_cam_op.grey_frac);
-      do_autofocus(&s->road_cam, &sm);
+      do_autofocus(&s->road_cam);
       last_road_cam_op_id = road_cam_op.op_id;
     }
 
@@ -1165,7 +1137,6 @@ void cameras_run(MultiCameraState *s) {
             .frame_length = (uint32_t)c->frame_length,
             .integ_lines = (uint32_t)c->cur_integ_lines,
             .lens_pos = c->cur_lens_pos,
-            .lens_sag = c->last_sag_acc_z,
             .lens_err = c->focus_err,
             .lens_true_pos = c->lens_true_pos,
             .gain = c->cur_gain_frac,

selfdrive/camerad/cameras/camera_qcom.h

@@ -76,7 +76,6 @@ typedef struct CameraState {
   // rear camera only,used for focusing
   unique_fd actuator_fd;
   std::atomic<float> focus_err;
-  std::atomic<float> last_sag_acc_z;
   std::atomic<float> lens_true_pos;
   std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
   uint16_t cur_step_pos;