Worked on self-calibration for Inga Nodes. Still got some bugs

software/main.c

@@ -35,6 +35,11 @@
 #include "net/gnrc/netif/ieee802154.h"
 #include "periph/i2c.h"
 
+#include "at86rf2xx.h"
+#include "at86rf2xx_params.h"
+#define ENABLE_DEBUG      (1)
+#include "debug.h"
+
 #define SEND_INTERVAL (1)
 #define RCV_QUEUE_SIZE (8)
 #define MAX_PAYLOAD_LENGTH (32)
@@ -56,13 +61,39 @@ char send_thread_stack[512+256];
 #endif
 
 #ifdef BOARD_TELOSB
-#include "board.h"
-#include "stdio_uart.h"
-extern void msp430_init_dco(void);
-char calib_thread_stack[512+256];
-#define CALIBRATION_SLEEP_TIME 1
+
+    #include "board.h"
+    #include "stdio_uart.h"
+    #define CALIBRATION_SLEEP_TIME 1
+    extern void msp430_init_dco(void);
+    char calib_thread_stack[512+256];
+
 #endif
 
+#if defined(BOARD_INGA_BLUE) || defined(BOARD_INGA_RED)
+
+    #include "board.h"
+    #include "stdio_uart.h"
+    //#define CLOCK_SECOND			1
+    //#define CLOCK_CALIB_WAIT()		_delay_us(1)
+
+    #define CLOCK_CALIB_FRQ_XTAL		32768UL
+    #define CLOCK_CALIB_FRQ_REF_CNT		(uint16_t)((CLOCK_CORECLOCK / CLOCK_CALIB_FRQ_XTAL) * 100)
+    #define CALIBRATION_SLEEP_TIME 1
+
+    char calib_thread_stack[512+256];
+
+    void clock_init_timer(void);
+
+    uint8_t clock_s(uint8_t seconds);
+
+    uint16_t clock_get_tcnt1(void);
+
+    void clock_calibrate(void);
+
+#endif
+
+
 //static msg_t dump_thread_msg_queue[RCV_QUEUE_SIZE];
 //static msg_t send_thread_msg_queue[RCV_QUEUE_SIZE];
 
@@ -149,33 +180,33 @@ int shell_memdump(int argc, char** argv) {
 }
 
 #ifdef BOARD_INGA_BLUE
-int shell_temp(int argc, char** argv) {
-    (void) argc;
-    (void) argv;
-
-    // uint8_t result;
-    uint8_t result[2];
-
-	i2c_acquire(0);
-    //i2c_read_reg(0, 0x48, 0x00, &result);
-	i2c_read_regs(0, 0x48, 0x00, result, 2, 0);
-	i2c_release(0);
-    //uint16_t temp = (result[0] << 8) | (result[1] >> 4);
-	//printf("temperature is %d %d\n", result[0], result[1]);
-    //printf("temperature=%u\n", temp);
-    printf("temperature=%u.%d\n", result[0], (result[1]>>4) );
-
-    return 0;
-}
+    int shell_temp(int argc, char** argv) {
+        (void) argc;
+        (void) argv;
+
+        // uint8_t result;
+        uint8_t result[2];
+
+        i2c_acquire(0);
+        //i2c_read_reg(0, 0x48, 0x00, &result);
+        i2c_read_regs(0, 0x48, 0x00, result, 2, 0);
+        i2c_release(0);
+        //uint16_t temp = (result[0] << 8) | (result[1] >> 4);
+        //printf("temperature is %d %d\n", result[0], result[1]);
+        //printf("temperature=%u\n", temp);
+        printf("temperature=%u.%d\n", result[0], (result[1]>>4) );
+
+        return 0;
+    }
 #endif
 
 shell_command_t eval_shell_commands[] = {
     {"tx", "Sends packets", shell_tx},
     {"payload", "Sets or prints the payload", shell_payload},
     {"memdump", "Prints the memory", shell_memdump},
-#ifdef BOARD_INGA_BLUE
+    #ifdef BOARD_INGA_BLUE
     {"temp", "Read temperatur", shell_temp},
-#endif
+    #endif
     {NULL, NULL, NULL}
 };
 
@@ -518,20 +549,312 @@ void *send_thread(void *arg)
 
 
 #ifdef BOARD_TELOSB
-void *calib_thread(void *arg)
-{
-	int *intPoint = (int*)arg;
-	printf("Got int: %d\n", *intPoint);
-	while(1)
-	{
-		xtimer_sleep(CALIBRATION_SLEEP_TIME);
-		msp430_init_dco();
-
-		/*calibrate xtimer and stdio again*/
-		xtimer_init();
-		stdio_init();
-	}
-}
+    void *calib_thread(void *arg) {
+        int *intPoint = (int*)arg;
+        printf("Got int: %d\n", *intPoint);
+        while(1)
+        {
+            xtimer_sleep(CALIBRATION_SLEEP_TIME);
+            msp430_init_dco();
+
+            /*calibrate xtimer and stdio again*/
+            xtimer_init();
+            stdio_init();
+        }
+    }
+#endif
+
+#if defined(BOARD_INGA_BLUE) || defined(BOARD_INGA_RED)
+
+    volatile uint8_t clock_flag = 0;
+
+    uint16_t clock_tcnt1 = 0;
+
+    void clock_init_timer(void){
+        /* Init Timer 2*/
+            
+        ASSR = (1 << AS2);
+        //TIMSK2 |= (1 << TOV2);
+        TCCR2B |= (1 << CS22) | (1 << CS20);
+        
+        /* Init Timer 1*/
+        TCCR1B |= (1 << CS12);			//prescaler = 256
+        OCR1A = 31248;//15624;//31248;	// bei 8mhz			//Compare Register is set to 1s
+        TIMSK1 |= (1 << OCIE1A);	//enable compare interrupt	
+        TCNT1 = 0;
+        sei();
+    }
+
+    uint8_t clock_s(uint8_t seconds){
+        if(clock_flag >= seconds){
+            clock_flag = 0;
+            return 1;
+        }else{
+            return 0;
+        }
+    }
+
+    uint16_t clock_get_tcnt1(void){
+        return clock_tcnt1; 
+    }
+
+    void clock_calibrate(void) {
+        cli();
+        uint8_t tries = 50;
+        uint8_t bak_timsk2, bak_tccr2b, bak_tccr1b, bak_assr;
+        uint16_t min = 0xFFFF;
+        uint8_t min_osccal = 0;
+        uint16_t ticks;
+        /* backup timer register */
+        bak_timsk2 = TIMSK2;
+        TIMSK2 = 0x00;
+        /* backup timer register */
+        bak_assr = ASSR;
+        ASSR = 0x00;
+        ASSR |= (1 << AS2); //CLK_SRC für TIMER2 ist externer Quarz
+        
+        while (ASSR & ((1 << TCN2UB)|(1 << OCR2AUB)|(1 << TCR2AUB)|(1<< TCR2BUB))){
+            ;
+        }
+        /* backup timer register */
+        bak_tccr2b = TCCR2B;
+        TCCR2B = 0x00;
+        bak_tccr1b = TCCR1B;
+        TCCR2B = 0x00;
+
+        TCCR2B = 1 << CS20; //No Prescaler
+        TCCR1B = 1 << CS10; //No Prescaler
+
+        //OSCCAL = 74;
+        /**
+        for (unsigned int i = 0xFFFF; i > 0; i--) {       
+        __asm__("nop");
+        }
+        **/
+        //uint8_t ticks2 = 0;
+        do {
+            /* get counter values */
+            ticks = 0;
+            TCNT2 = 0;
+            TCNT1 = 0;
+            //OC2RA = 100;
+            while (TCNT2 < 100) { 
+                ticks = TCNT1;
+            }
+            ticks = TCNT1; 
+            //ticks2 = TCNT2;
+            //DEBUG("Counted Ticks: %ul\n", ticks);
+            //DEBUG("Ref Ticks: %d\n", ticks2);
+            
+            if (ticks > CLOCK_CALIB_FRQ_REF_CNT) {
+                if((ticks - CLOCK_CALIB_FRQ_REF_CNT) < min){
+                    min_osccal = OSCCAL;
+                    //DEBUG("Seting min_osccal to: %#08x\n", OSCCAL);
+                }
+                //DEBUG("Tuning down. Current Reg Val: %#08x\n", OSCCAL);
+                OSCCAL--;
+            } else if (ticks < CLOCK_CALIB_FRQ_REF_CNT) {
+                if((CLOCK_CALIB_FRQ_REF_CNT - ticks) < min){
+                    min_osccal = OSCCAL;
+                    //DEBUG("Seting min_osccal to: %#08x\n", OSCCAL);
+                }
+                //DEBUG("Tuning up. Current Reg Val: %#08x\n", OSCCAL);
+                OSCCAL++;
+            } else if (ticks == CLOCK_CALIB_FRQ_REF_CNT) {
+                //DEBUG("Perfect. Ending calib. Attempts left: %d\n", tries);
+                tries = 1;
+                min_osccal = OSCCAL;
+            }
+
+            for (unsigned int i = 0x2FFF; i > 0; i--) {       
+                __asm__("nop");
+            }
+            
+            
+        }while (--tries);
+
+        OSCCAL = min_osccal;
+        DEBUG("Setting OSCCAl to Value %d\n", OSCCAL);
+        
+        for (unsigned int i = 0xFF; i > 0; i--) {       
+            __asm__("nop");
+        }
+        DEBUG("Counted Ticks: %ul\n", ticks);
+        //CLOCK_CALIB_WAIT();
+        /* restore registers */
+        ASSR = bak_assr;
+        TIMSK2 = bak_timsk2;
+        TCCR2B = bak_tccr2b;
+        TCCR1B = bak_tccr1b;
+
+        for (unsigned int i = 0xFFFF; i > 0; i--) {        /* Delay for XTAL to settle */
+            __asm__("nop");
+        }
+        sei();
+    }
+
+
+    void clock_calibrate_2(void) {
+        at86rf2xx_t at86rf2xx_dev;
+        at86rf2xx_setup(&at86rf2xx_dev, &at86rf2xx_params[0]);
+        cli();
+        at86rf233_disable_irq(&at86rf2xx_dev);
+
+        uint8_t tries = 50;
+        uint8_t bak_timsk2, bak_tccr2b, bak_tccr2a, bak_tccr1b, bak_assr, bak_ddrd, bak_portd;
+        //uint16_t min = 0xFFFF;
+        //uint8_t min_osccal = 0;
+        uint16_t ticks;
+
+        /* backup timer register */
+        bak_timsk2 = TIMSK2;
+        TIMSK2 = 0x00;
+        /* backup timer register */
+        bak_assr = ASSR;
+        ASSR = 0x00;
+        ASSR |= (1 << AS2); //CLK_SRC für TIMER2 ist externer Quarz
+        
+        while (ASSR & ((1 << TCN2UB)|(1 << OCR2AUB)|(1 << TCR2AUB)|(1<< TCR2BUB))){
+            ;
+        }
+        /* backup timer register */
+        bak_tccr2b = TCCR2B;
+        TCCR2B = 0x00;
+        bak_tccr2a = TCCR2A;
+        TCCR2A = 0x00; //set OC2B off
+        bak_tccr1b = TCCR1B;
+        TCCR1B = 0x00;
+
+        TCCR1B |= 1 << ICES1; //Activate Input Capture on positive/rising edge
+        TCCR1B |= 1 << ICNC1; //Activate Noise Canceler
+        OCR2B = 100;
+        //set D6 to output and set on 0. Save D6
+        bak_ddrd = DDRD;
+        bak_portd = PORTD;
+        DDRD |= 1 << PD6;
+        PORTD &= ~(1 << PD6);
+
+        TCCR1B |= 1 << CS10; //No Prescaler
+
+        DEBUG("PIND init: %d\n", PIND);
+        do {
+            //clear Flag for ICF1
+            if(TIFR1 & (1 << ICF1)) {
+                TIFR1 |= 1 << ICF1;
+            }
+
+            //clear Flag for OCR2B
+            if(TIFR2 & (1 << OCF2B)){
+                TIFR2 |= 1 << OCF2B;
+            }
+            
+            PORTD &= (~(1 << PD6)); /*Set pin D6 back to 0 */
+            //DEBUG("Entering while, %d %d\n", TIFR2, TIFR1);
+            //DEBUG("PIND before: %d\n", PIND);
+            
+            TCCR2A = 0x30; //set OC2B on Compare Match
+            
+            TCNT2 = 0;
+            //DEBUG("PIND before while: %d\n", PIND);
+
+            //Turn on Timer
+            //TCCR1B |= 1 << CS10; //No Prescaler
+            TCNT1 = 0;
+            TCCR2B |= 1 << CS20; //No Prescaler
+            
+            while ((!(TIFR2 & (1 << OCF2B))) || (!(TIFR1 & (1 << ICF1)))); /* Check for both flags */
+                /**
+                DEBUG("Reg T2 Val: %d\n", TIFR2);
+                DEBUG("Reg T1 Val: %d\n", TIFR1);
+                DEBUG("DDRD: %d\n", DDRD);
+                DEBUG("PIND: %d\n", PIND);
+                DEBUG("TCCR1A: %d\n", TCCR1A);
+                DEBUG("TCCR1B: %d\n", TCCR1B);
+                DEBUG("TCCR2A: %d\n", TCCR2A);
+                DEBUG("TCCR2B: %d\n", TCCR2B); 
+                DEBUG("-------->\n");
+                **/
+            ticks = ICR1;
+            TCCR2B &= 0b11111000; //Off
+            //TCCR1B &= 0b11111000; //Off
+            //DEBUG("Count: %d\n", TCNT2);
+            //DEBUG("PIND before: %d\n", PIND);
+            TCCR2A = 0x00; //set OC2B into Port-Mode
+            //DEBUG("Reg T2 Val after flag2: %d\n", TIFR2);
+            //DEBUG("Reg T1 Val after flag2: %d\n", TIFR1);
+            //DEBUG("PIND after: %d\n", PIND);
+            DEBUG("Counted Ticks: %ul\n", ticks);
+            DEBUG("----->\n");
+
+            /*CLOCK_CALIB_FRQ_REF_CNT = 24400*/
+            if (ticks > CLOCK_CALIB_FRQ_REF_CNT) {
+                /**
+                if((ticks - CLOCK_CALIB_FRQ_REF_CNT) < min){
+                    min_osccal = OSCCAL;
+                }
+                **/
+                //OSCCAL--;
+            } else if (ticks < CLOCK_CALIB_FRQ_REF_CNT) {
+                /**
+                if((CLOCK_CALIB_FRQ_REF_CNT - ticks) < min){
+                    min_osccal = OSCCAL;
+                }
+                **/
+                //OSCCAL++;
+            } else if (ticks == CLOCK_CALIB_FRQ_REF_CNT) {
+                tries = 1;
+                //min_osccal = OSCCAL;
+            }
+            /**
+            for (unsigned int i = 0xFFFF; i > 0; i--) {       
+                __asm__("nop");
+            }
+            for (unsigned int i = 0xFFFF; i > 0; i--) {       
+                __asm__("nop");
+            }
+            **/
+        }while (--tries);
+
+        //OSCCAL = min_osccal;
+        DEBUG("Setting OSCCAl to Value %d\n", OSCCAL);
+        
+        for (unsigned int i = 0xFF; i > 0; i--) {       
+            __asm__("nop");
+        }
+        DEBUG("Counted Ticks: %ul\n", ticks);
+        
+        /* restore registers */
+        ASSR = bak_assr;
+        TIMSK2 = bak_timsk2;
+        TCCR2A = bak_tccr2a;
+        TCCR2B = bak_tccr2b;
+        TCCR1B = bak_tccr1b;
+        DDRD = bak_ddrd;
+        PORTD = bak_portd;
+
+        for (unsigned int i = 0xFFFF; i > 0; i--) {        /* Delay for XTAL to settle */
+            __asm__("nop");
+        }
+        sei();
+        at86rf233_enable_irq(&at86rf2xx_dev);
+    }
+
+    void *calib_thread(void *arg) {
+        int *intPoint = (int*)arg;
+        printf("Got int: %d\n", *intPoint);
+        
+        while(1)
+        {
+            xtimer_sleep(CALIBRATION_SLEEP_TIME);
+            clock_calibrate_2();
+
+            /*calibrate xtimer and stdio again*/
+            xtimer_init();
+            stdio_init();
+            DEBUG("calib complete\n");
+        }
+    }
+
 #endif
 
 int main(void)
@@ -542,13 +865,20 @@ int main(void)
 
     //int res = i2c_init(I2C_DEV(0));
     #if defined(BOARD_INGA_BLUE) || defined(BOARD_INGA_RED)
-    puts("This is an inga");
-    i2c_init(I2C_DEV(0));
+        puts("This is an inga");
+        //Get dev for Radio
+        at86rf2xx_t at86rf2xx_dev;
+        at86rf2xx_setup(&at86rf2xx_dev, &at86rf2xx_params[0]);
+        at86rf233_disable_irq(&at86rf2xx_dev);
+        int i = 5;
+        thread_create(calib_thread_stack, sizeof(calib_thread_stack), 1, THREAD_CREATE_STACKTEST, calib_thread, &i, "calib_thread");
+        i2c_init(I2C_DEV(0));
     #endif
+
     #if defined (BOARD_TELOSB)
-    puts("This is a telosb");
-    int i = 5;
-	thread_create(calib_thread_stack, sizeof(calib_thread_stack), 1, THREAD_CREATE_STACKTEST, calib_thread, &i, "calib_thread");
+        puts("This is a telosb");
+        int i = 5;
+        thread_create(calib_thread_stack, sizeof(calib_thread_stack), 1, THREAD_CREATE_STACKTEST, calib_thread, &i, "calib_thread");
     #endif
     //printf("i2c init: %s\n", res == 0 ? "OK\n" : "FAILED\n");
     
@@ -569,7 +899,9 @@ int main(void)
     printf("line_buf from %p to %p\n", line_buf, line_buf + sizeof(line_buf));
     printf("serialbuffer: from %p to %p\n", serialbuffer, serialbuffer+sizeof(serialbuffer));
     printf("serialbuffer2: from %p to %p\n", serialbuffer2, serialbuffer2+sizeof(serialbuffer2));
-
+    printf("OSCCAL Val: %d\n", OSCCAL);
+    at86rf233_disable_irq(&at86rf2xx_dev);
+    //at86rf233_enable_irq(&at86rf2xx_dev);
     shell_run(eval_shell_commands, line_buf, SHELL_BUFFER_SIZE);
 
     return 0;