WebSVN – svnkaklik – Diff – /programy/C/avr32/cmps03/compass.c


/* 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);
   }
}

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

Subversion Repositories svnkaklik

(root)/programy/C/avr32/cmps03/compass.c – Rev 409 → 410