1 files changed, 64 insertions, 0 deletions

diff --git a/firmware/libs-device/osccal.c b/firmware/libs-device/osccal.c
index b8d1bb5..8debe49 100644
--- a/firmware/libs-device/osccal.c
+++ b/firmware/libs-device/osccal.c
@@ -8,6 +8,7 @@
  */
 
 #include <avr/io.h>
+#include <avr/interrupt.h>
 #ifndef uchar
 #define uchar   unsigned char
 #endif
@@ -59,6 +60,69 @@ int usbMeasureFrameLengthDecreasing(int);
 void    calibrateOscillator(void)
 {
 	uchar       step, trialValue, optimumValue;
+	int         x, targetValue;
+	uchar		optimumDev;
+	uchar		i,xl;
+	
+	targetValue = (unsigned)((double)F_CPU * 999e-6 / 5.0 + 0.5);  /* Time is measured in multiples of 5 cycles. Target is 0.999µs */
+    optimumDev = 0xff;   
+  //  optimumValue = OSCCAL; 
+	step=64;
+	trialValue = 128;
+	
+	cli(); // disable interrupts
+	
+	/*
+		Performs seven iterations of a binary search (stepwidth decreasing9
+		with three additional steps of a neighborhood search (step=1, trialvalue will oscillate around target value to find optimum)
+	*/
+
+	for(i=0; i<10; i++){
+		OSCCAL = trialValue;
+		asm volatile(" NOP");
+	
+		x = usbMeasureFrameLengthDecreasing(targetValue);
+
+		if(x < 0)             /* frequency too high */
+		{
+			trialValue -= step;
+			xl=(uchar)-x;
+		}
+		else                  /* frequency too low */
+		{
+			trialValue += step;			
+			xl=(uchar)x;
+		}
+		
+		/*
+			Halve stepwidth to perform binary search. Logical oring with 1 to ensure step is never equal to zero. 
+			This results in a neighborhood search with stepwidth 1 after binary search is finished.			
+			Once the neighbourhood search stage is reached, x will be smaller than +-255, hence more code can be
+			saved by only working with the lower 8 bits.
+		*/
+		
+		step >>= 1;
+		
+		if (step==0)
+		{
+			step=1;			
+			if(xl <= optimumDev){
+				optimumDev = xl;
+				optimumValue = OSCCAL;
+			}
+		}
+
+	}
+
+	OSCCAL = optimumValue;
+	asm volatile(" NOP");
+	
+	sei(); // enable interrupts
+}
+
+void    calibrateOscillator_old(void)
+{
+	uchar       step, trialValue, optimumValue;
 	int         x, optimumDev, targetValue;
 	uchar		i;