Subversion Repositories svnkaklik

/* compass.c
 *
 * Software to read from the CMPS03 magnetic compass. 
 *
 
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/i2c-dev.h>

/* Note that the documentation for the compass states its address as 0xC0.
 * However, this includes the low bit which specifies read or write.
 * Linux i2c does not include this bit in this address, so the actual
 * address is 0xC0 shifted down, 0x60.
 */

#define CMPS_Addr       (0xC0>>1)
#define I2C_SLAVE       0x0B    /* Change slave address 

/* The important registers on the compass. Internal/test registers omitted. */
#define CMPS03_SOFTWARE_REVISION 0x0
#define CMPS03_BEARING_BYTE 0x1
#define CMPS03_BEARING_WORD_HIGH 0x2
#define CMPS03_BEARING_WORD_LOW 0x3
#define CMPS03_CALIBRATE_CMD 0xF

void I2C_addr (int Addr)  // vybere adresu cidla se kterym se bude komunikovat
{
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
    {
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
        exit(-5);
    }
}

int i2c_init()   // zinicializuje i2c
{
        file = open("/dev/i2c-0", O_RDWR);
        if (file < 0)
        {
                cerr << "Could not open /dev/i2c-0." << endl;
                return -1;
        }
        return 0;
}

unsigned char read_azimut_mag()  // precte azimut z kompasu
{
    char Buf[3];      // promena pro manipulaci s i2c

    I2C_addr(CMPS_Addr);
    Buf[0]=1;
    write(file,Buf,1);
    read(file, Buf,1);
    return (Buf[0]-SEVER);
}

void calib()  // kalibrace kompasu
{
    char Buf[3];      // promena pro manipulaci s i2c

    I2C_addr(CMPS_Addr);
    Buf[0]=15;
    Buf[1]=0xFF;
    write(file,Buf,2);
}

int main(int argc, char *argv[]) 
{
   char *end;
   int res,file;
   int error;
   char filename[20] ;
   long funcs;
    
   int heading_byte, heading_word_h, heading_word_l;
   int bearing_long, bearing_degrees;
   
   sprintf(filename,"/dev/i2c-0");
   file = open(filename,O_RDWR);
   if (errno != 0)
   {
     switch (errno)
     {
     case EACCES:
         fprintf(stderr,"Run as root? \n");
         break;
     default:
         fprintf(stderr,"Error: Could not open file '%s' : %s \n", filename, strerror(errno));
         break;
     }
   } 
      
   if (ioctl(file,I2C_SLAVE,CMPS03_ADDR) < 0) {
     switch (errno) 
     {
     case EBUSY:  
         printf("device is busy \n");
         break;

     default:
         printf("Got error: %s \n", strerror(errno));
         exit(0);
      }
   }
   
   /* Get software revision number */
   res = read(file, CMPS03_SOFTWARE_REVISION,1);
   if (res < 0) {
     printf("Cannot read software revision level \n");
   } else {
     printf("Software revision level: %02x \n", res);
   }
   
   /* Loop and read from the compass. */
   while (1) {
     /* The heading byte is 0-255 for the 360 degrees. */
     heading_byte = read(file, CMPS03_BEARING_BYTE,1);
     if (heading_byte < 0) { printf("Error reading from compass. \n"); exit(1);}
     
     /* The high resolution heading is given in two registers, and is 10 * the 
      * heading in degrees, ie 359.9 degrees reads as 3599. */
     heading_word_h = read(file, CMPS03_BEARING_WORD_HIGH,1);
     if (heading_word_h < 0) { printf("Error reading from compass. \n"); exit(1);}
     heading_word_l = read(file, CMPS03_BEARING_WORD_LOW,1);
     if (heading_word_l < 0) { printf("Error reading from compass. \n"); exit(1);}
     
     /* Combine the two bytes, and get the heading in degrees. */
     bearing_long = heading_word_h * 256 + heading_word_l;
     bearing_degrees = bearing_long / 10;
     
     printf("Bearing: %d \n", bearing_degrees);
     
     /* Wait for a while. */
     usleep(200000);
   }
}