| 
						
						
							
								
							
						
						
					 | 
					 | 
					@ -129,7 +129,7 @@ class Calibrator(): | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      self.cal_status = Calibration.INVALID | 
					 | 
					 | 
					 | 
					      self.cal_status = Calibration.INVALID | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    # If spread is too high, assume mounting was changed and reset to last block. | 
					 | 
					 | 
					 | 
					    # If spread is too high, assume mounting was changed and reset to last block. | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    # Make the transition smooth. Abrupt transitions are not good foor feedback loop through supercombo model. | 
					 | 
					 | 
					 | 
					    # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. | 
				
			
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: | 
					 | 
					 | 
					 | 
					    if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy) | 
					 | 
					 | 
					 | 
					      self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
	
		
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
					 | 
					@ -146,7 +146,7 @@ class Calibrator(): | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    else: | 
					 | 
					 | 
					 | 
					    else: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      return self.rpy | 
					 | 
					 | 
					 | 
					      return self.rpy | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  def handle_cam_odom(self, trans, rot, trans_std, rot_std): | 
					 | 
					 | 
					 | 
					  def handle_cam_odom(self, trans, rot, trans_std): | 
				
			
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) | 
					 | 
					 | 
					 | 
					    self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) | 
					 | 
					 | 
					 | 
					    straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
					 | 
					@ -213,8 +213,7 @@ def calibrationd_thread(sm=None, pm=None) -> NoReturn: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      calibrator.handle_v_ego(sm['carState'].vEgo) | 
					 | 
					 | 
					 | 
					      calibrator.handle_v_ego(sm['carState'].vEgo) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, | 
					 | 
					 | 
					 | 
					      new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					                                           sm['cameraOdometry'].rot, | 
					 | 
					 | 
					 | 
					                                           sm['cameraOdometry'].rot, | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					                                           sm['cameraOdometry'].transStd, | 
					 | 
					 | 
					 | 
					                                           sm['cameraOdometry'].transStd) | 
				
			
			
				
				
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					                                           sm['cameraOdometry'].rotStd) | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      if DEBUG and new_rpy is not None: | 
					 | 
					 | 
					 | 
					      if DEBUG and new_rpy is not None: | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        print('got new rpy', new_rpy) | 
					 | 
					 | 
					 | 
					        print('got new rpy', new_rpy) | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
					 | 
					
  |