Check sample time stamps in drift calculation.
Before calculating the clock drift in the PI servo, make sure the first sample is older than the second sample to avoid getting invalid drift or NaN. Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
This commit is contained in:
		
							parent
							
								
									a41e9f8c8c
								
							
						
					
					
						commit
						5667ab5d30
					
				
							
								
								
									
										6
									
								
								pi.c
									
									
									
									
									
								
							
							
						
						
									
										6
									
								
								pi.c
									
									
									
									
									
								
							@ -73,6 +73,12 @@ static double pi_sample(struct servo *servo,
 | 
			
		||||
		s->offset[1] = offset;
 | 
			
		||||
		s->local[1] = local_ts;
 | 
			
		||||
 | 
			
		||||
		/* Make sure the first sample is older than the second. */
 | 
			
		||||
		if (s->local[0] >= s->local[1]) {
 | 
			
		||||
			s->count = 0;
 | 
			
		||||
			break;
 | 
			
		||||
		}
 | 
			
		||||
 | 
			
		||||
		s->drift += (s->offset[1] - s->offset[0]) * 1e9 /
 | 
			
		||||
			(s->local[1] - s->local[0]);
 | 
			
		||||
		if (s->drift < -s->maxppb)
 | 
			
		||||
 | 
			
		||||
		Loading…
	
	
			
			x
			
			
		
	
		Reference in New Issue
	
	Block a user