Track calib spread (#1988)

* add calib spread metric

* add field

* needs to be array

* update refs

* log valid blocks too

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/locationd/calibrationd.py

@@ -126,13 +126,21 @@ class Calibrator():
 
   def send_data(self, pm):
     calib = get_calib_from_vp(self.vp)
+    if self.valid_blocks > 0:
+      max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0)))
+      min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0)))
+      calib_spread = np.abs(max_vp_calib - min_vp_calib)
+    else:
+      calib_spread = np.zeros(3)
     extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
 
     cal_send = messaging.new_message('liveCalibration')
+    cal_send.liveCalibration.validBlocks = self.valid_blocks
     cal_send.liveCalibration.calStatus = self.cal_status
     cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
     cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
     cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
+    cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread]
 
     pm.send('liveCalibration', cal_send)
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-3e8932a5dc0281b00463d40a5435f659259591c4
+158743f002e82b62a67c6b91308db01d715f1643