#include <pic.h>
#include "hardware.h"
#define __DEBUG
void init_i2c(void)  {
    char x ;
    #ifdef __DEBUG
    TRISB5=0;       // RA4 reflete l'état du dernier ACK du SONAR
    RB5 = 0 ;
    #define ACKLED RB5  
    #endif
    TRISC3 = 1 ; //SDA et SCL en entrée
    TRISC4 = 1 ;
    SSPSTAT = 0x80 ; // Pas de controle de slew rate, niveaux i2c
    SSPCON = 0x28 ; // enable i2c 7bits master mode
    SSPADD =  9 ; // fclock = 100khz = Fosc/(4(SSPADD+1)
    x = SSPBUF ; // vide le buffer et raz flags
   }

unsigned int read_compass(void)
 {  
    char bh,bl;
    unsigned int b ;
	SEN = 1;						// send start bit
	while(SEN);					// and wait for it to clear
	ACKDT = 0;					// acknowledge bit
    
    ACKSTAT=1;
	SSPIF = 0;
	SSPBUF = 0xC0;				// C0 - write command
	while(!SSPIF);				// wait for interrupt
	SSPIF = 0;
    #ifdef __DEBUG					
    ACKLED=ACKSTAT;
    ACKSTAT=1;
    #endif
    SSPIF = 0;
	SSPBUF = 0x02;				// adress of cmd register
	while(!SSPIF);				// wait for interrupt
	SSPIF = 0;	
     #ifdef __DEBUG					
    ACKLED=ACKSTAT;
    ACKSTAT=1;
    #endif
	PEN = 1;						// send stop bit
	while(PEN);					//

   
    SEN = 1;					// send start bit
	while(SEN);					// and wait for it to clear
	ACKDT = 0;					// acknowledge bit
    
    SSPIF = 0;
	SSPBUF = 0xC1;				// 11000001 - read command
	while(!SSPIF);				// wait for interrupt
	SSPIF = 0;					// then clear it.
		
	RCEN = 1;					// start receiving
	while(!STAT_BF);			// wait for data
	bh = SSPBUF;		// and get it
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end
	
    RCEN = 1;					// start receiving
	while(!STAT_BF);			// wait for data
	bl = SSPBUF;		// and get it
    ACKDT=1;                    // very important ??
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end
  
    PEN = 1;						// send stop bit
	while(PEN);					//
     #ifdef __DEBUG					
    ACKLED=1;
     #endif  

    b = (bh<<8) + bl ;
    
    return b ;
   
  
}