#include <pic.h>


void init_compass(void)
{
char x;


	TRISC = 0x18;			// RC3-4 are inputs
	SSPSTAT = 0x80;		// slew rate disabled
	SSPCON  = 0x38;		// enable port in 7 bit slave mode
	SSPCON2 = 0x00;		// not a lot
	SSPADD  = 19;			// baud rate in address reg
	x = SSPBUF;				// dummy read clears BF
}

void read_compass(unsigned char *buffer)
{
	SEN = 1;						// send start bit
	while(SEN);					// and wait for it to clear
	ACKDT = 0;					// acknowledge bit

	SSPIF = 0;
	SSPBUF = 0xC0;				// 11000000 - write command
	while(!SSPIF);				// wait for interrupt
	SSPIF = 0;					// then clear it.

	SSPBUF = 0;					// read from address 0 
	while(!SSPIF);				// 
	SSPIF = 0;					// 

	RSEN = 1;					// send repeated start bit
	while(RSEN);				// and wait for it to clear

	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
	buffer[0] = 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
	buffer[1] = 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
	buffer[2] = 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
	buffer[3] = SSPBUF;		// and get it
	ACKDT = 1;					// not acknowledge for last byte
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end

	PEN = 1;						// send stop bit
	while(PEN);					//
}
/* retourne la direction courante 0-3599 <-> 0° - 359.9° */
int read_bearing(void)
{
	char b_lsb,b_msb;
	SEN = 1;						// send start bit
	while(SEN);					// and wait for it to clear
	ACKDT = 0;					// acknowledge bit

	SSPIF = 0;
	SSPBUF = 0xC0;				// 11000000 - write command
	while(!SSPIF);				// wait for interrupt
	SSPIF = 0;					// then clear it.

	SSPBUF = 2;					// read from address 2 
	while(!SSPIF);				// 
	SSPIF = 0;					// 

	RSEN = 1;					// send repeated start bit
	while(RSEN);				// and wait for it to clear

	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
	b_msb = 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
	b_lsb = SSPBUF;	       		// and get it
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end

	PEN = 1;					// send stop bit
	while(PEN);					//
	
	return (int)((b_msb<<8) + b_lsb) ;
}
/* retourne la direction courante 0-3599 <-> 0° - 359.9° */
/* avec gestion time out*/
#define COMPASS_TIME_OUT 10000
int read_bearing2(void)
{
	char b_lsb,b_msb;
    unsigned int tout;
	SEN = 1;						// send start bit
	while(SEN);					// and wait for it to clear
	ACKDT = 0;					// acknowledge bit

	SSPIF = 0;
	SSPBUF = 0xC0;				// 11000000 - write command
    tout=0;
	while(!SSPIF)				// wait for interrupt or time out
     {
       tout++;
       if (tout>COMPASS_TIME_OUT)
        {
          return COMPASS_TIME_OUT ;
        }
     }
	SSPIF = 0;					// then clear it.

	SSPBUF = 2;					// read from address 2 
   tout=0;
	while(!SSPIF)				// wait for interrupt or time out
     {
       tout++;
       if (tout>COMPASS_TIME_OUT)
        {
          return COMPASS_TIME_OUT ;
        }
     }			// 
	SSPIF = 0;					// 

	RSEN = 1;					// send repeated start bit
	while(RSEN);				// and wait for it to clear

	SSPIF = 0;
	SSPBUF = 0xC1;				// 11000001 - read command
 tout=0;
	while(!SSPIF)				// wait for interrupt or time out
     {
       tout++;
       if (tout>COMPASS_TIME_OUT)
        {
          return COMPASS_TIME_OUT ;
        }
     }
	SSPIF = 0;					// then clear it.
		
	RCEN = 1;					// start receiving
    tout=0;
	while(!STAT_BF)			// wait for data or time out
     {
       tout++;
       if (tout>COMPASS_TIME_OUT)
        {
          return COMPASS_TIME_OUT ;
        }
     }
	b_msb = SSPBUF;				// and get it
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end
	
	RCEN = 1;					// start receiving
	 tout=0;
	while(!STAT_BF)			// wait for data or time out
     {
       tout++;
       if (tout>COMPASS_TIME_OUT)
        {
          return COMPASS_TIME_OUT ;
        }
     }
	b_lsb = SSPBUF;	       		// and get it
	ACKEN = 1;					// start acknowledge sequence
	while(ACKEN);				// wait for ack. sequence to end

	PEN = 1;					// send stop bit
	while(PEN);					//
	
	return (int)((b_msb<<8) + b_lsb) ;
}