Subversion Repositories svnkaklik

Rev

Rev 409 | Details | Compare with Previous | Last modification | View Log

Rev Author Line No. Line
385 kakl 1 
/* compass.c
2 
 *
3 
 * Software to read from the CMPS03 magnetic compass.
4 
 *
5 
 
6 
#include <errno.h>
7 
#include <string.h>
8 
#include <stdio.h>
9 
#include <stdlib.h>
10 
#include <unistd.h>
11 
#include <fcntl.h>
12 
#include <linux/i2c-dev.h>
13 
 
14 
/* Note that the documentation for the compass states its address as 0xC0.
15 
 * However, this includes the low bit which specifies read or write.
16 
 * Linux i2c does not include this bit in this address, so the actual
17 
 * address is 0xC0 shifted down, 0x60.
18 
 */
19 
 
20 
#define CMPS_Addr	(0xC0>>1)
21 
#define I2C_SLAVE	0x0B	/* Change slave address
22 
 
23 
/* The important registers on the compass. Internal/test registers omitted. */
24 
#define CMPS03_SOFTWARE_REVISION 0x0
25 
#define CMPS03_BEARING_BYTE 0x1
26 
#define CMPS03_BEARING_WORD_HIGH 0x2
27 
#define CMPS03_BEARING_WORD_LOW 0x3
28 
#define CMPS03_CALIBRATE_CMD 0xF
29 
 
30 
void I2C_addr (int Addr)  // vybere adresu cidla se kterym se bude komunikovat
31 
{
32 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
33 
    {
34 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
35 
        exit(-5);
36 
    }
37 
}
354 kaklik 38 
 
385 kakl 39 
int i2c_init()   // zinicializuje i2c
40 
{
41 
	file = open("/dev/i2c-0", O_RDWR);
42 
	if (file < 0)
43 
	{
44 
		cerr << "Could not open /dev/i2c-0." << endl;
45 
		return -1;
46 
	}
47 
	return 0;
48 
}
49 
 
50 
unsigned char read_azimut_mag()  // precte azimut z kompasu
51 
{
52 
    char Buf[3];      // promena pro manipulaci s i2c
354 kaklik 53 
 
385 kakl 54 
    I2C_addr(CMPS_Addr);
55 
    Buf[0]=1;
56 
    write(file,Buf,1);
57 
    read(file, Buf,1);
58 
    return (Buf[0]-SEVER);
59 
}
354 kaklik 60 
 
385 kakl 61 
void calib()  // kalibrace kompasu
62 
{
63 
    char Buf[3];      // promena pro manipulaci s i2c
354 kaklik 64 
 
385 kakl 65 
    I2C_addr(CMPS_Addr);
66 
    Buf[0]=15;
67 
    Buf[1]=0xFF;
68 
    write(file,Buf,2);
69 
}
70 
 
71 
int main(int argc, char *argv[])
72 
{
73 
   char *end;
74 
   int res,file;
75 
   int error;
76 
   char filename[20] ;
77 
   long funcs;
78 
 
79 
   int heading_byte, heading_word_h, heading_word_l;
80 
   int bearing_long, bearing_degrees;
81 
 
82 
   sprintf(filename,"/dev/i2c-0");
358 kaklik 83 
   file = open(filename,O_RDWR);
84 
   if (errno != 0)
385 kakl 85 
   {
358 kaklik 86 
     switch (errno)
385 kakl 87 
     {
88 
     case EACCES:
358 kaklik 89 
         fprintf(stderr,"Run as root? \n");
385 kakl 90 
         break;
91 
     default:
358 kaklik 92 
         fprintf(stderr,"Error: Could not open file '%s' : %s \n", filename, strerror(errno));
93 
         break;
385 kakl 94 
     }
95 
   }
96 
 
97 
   if (ioctl(file,I2C_SLAVE,CMPS03_ADDR) < 0) {
358 kaklik 98 
     switch (errno)
385 kakl 99 
     {
358 kaklik 100 
     case EBUSY:
101 
         printf("device is busy \n");
102 
         break;
103 
 
385 kakl 104 
     default:
105 
         printf("Got error: %s \n", strerror(errno));
358 kaklik 106 
         exit(0);
385 kakl 107 
      }
108 
   }
109 
 
110 
   /* Get software revision number */
111 
   res = read(file, CMPS03_SOFTWARE_REVISION,1);
112 
   if (res < 0) {
113 
     printf("Cannot read software revision level \n");
114 
   } else {
115 
     printf("Software revision level: %02x \n", res);
116 
   }
117 
 
118 
   /* Loop and read from the compass. */
119 
   while (1) {
120 
     /* The heading byte is 0-255 for the 360 degrees. */
121 
     heading_byte = read(file, CMPS03_BEARING_BYTE,1);
122 
     if (heading_byte < 0) { printf("Error reading from compass. \n"); exit(1);}
123 
 
124 
     /* The high resolution heading is given in two registers, and is 10 * the
125 
      * heading in degrees, ie 359.9 degrees reads as 3599. */
126 
     heading_word_h = read(file, CMPS03_BEARING_WORD_HIGH,1);
127 
     if (heading_word_h < 0) { printf("Error reading from compass. \n"); exit(1);}
128 
     heading_word_l = read(file, CMPS03_BEARING_WORD_LOW,1);
129 
     if (heading_word_l < 0) { printf("Error reading from compass. \n"); exit(1);}
130 
 
131 
     /* Combine the two bytes, and get the heading in degrees. */
132 
     bearing_long = heading_word_h * 256 + heading_word_l;
133 
     bearing_degrees = bearing_long / 10;
134 
 
135 
     printf("Bearing: %d \n", bearing_degrees);
136 
 
137 
     /* Wait for a while. */
138 
     usleep(200000);
139 
   }
140 
}