| 354 |
kaklik |
1 |
/* compass.c
|
|
|
2 |
*
|
|
|
3 |
* Software to read from the CMPS03 magnetic compass.
|
|
|
4 |
*
|
|
|
5 |
* By Josh Marshall, joshua dot marshall at studentmail dot newcastle dot edu dot au
|
|
|
6 |
* October 15, 2004.
|
|
|
7 |
*
|
|
|
8 |
* Adapted from code for the SP03 speech synthesiser
|
|
|
9 |
* By Bram Stolk, b.stolk at chello.nl
|
|
|
10 |
*
|
|
|
11 |
* Bram Stolk's notes:
|
|
|
12 |
* In linux, you can do I2C stuff in user-space, if you enable
|
|
|
13 |
* the device-interface to i2c. You can use plain read() and
|
|
|
14 |
* write() calls on your /dev/i2c-0 file.
|
|
|
15 |
*
|
|
|
16 |
* NOTE: Check if you have an i2c adapter active by doing:
|
|
|
17 |
* cat /proc/bus/i2c
|
|
|
18 |
* If this comes up empty, then you have no adapters. If the
|
|
|
19 |
* file does not exist at all, you probably lack i2c kernel
|
|
|
20 |
* support alltogether.
|
|
|
21 |
*/
|
|
|
22 |
|
|
|
23 |
#include <errno.h>
|
|
|
24 |
#include <string.h>
|
|
|
25 |
#include <stdio.h>
|
|
|
26 |
#include <stdlib.h>
|
|
|
27 |
#include <unistd.h>
|
|
|
28 |
#include <fcntl.h>
|
|
|
29 |
#include <linux/i2c-dev.h>
|
|
|
30 |
|
|
|
31 |
/* Note that the documentation for the compass states its address as 0xC0.
|
|
|
32 |
* However, this includes the low bit which specifies read or write.
|
|
|
33 |
* Linux i2c does not include this bit in this address, so the actual
|
|
|
34 |
* address is 0xC0 shifted down, 0x60.
|
|
|
35 |
*/
|
|
|
36 |
#define CMPS03_ADDR 0x60
|
|
|
37 |
|
|
|
38 |
/* The important registers on the compass. Internal/test registers omitted. */
|
|
|
39 |
#define CMPS03_SOFTWARE_REVISION 0x0
|
|
|
40 |
#define CMPS03_BEARING_BYTE 0x1
|
|
|
41 |
#define CMPS03_BEARING_WORD_HIGH 0x2
|
|
|
42 |
#define CMPS03_BEARING_WORD_LOW 0x3
|
|
|
43 |
#define CMPS03_CALIBRATE_CMD 0xF
|
|
|
44 |
|
|
|
45 |
int main(int argc, char *argv[]) {
|
|
|
46 |
char *end;
|
|
|
47 |
int res,file;
|
|
|
48 |
int e1;
|
|
|
49 |
char filename[20] ;
|
|
|
50 |
long funcs;
|
|
|
51 |
|
|
|
52 |
int heading_byte, heading_word_h, heading_word_l;
|
|
|
53 |
int bearing_long, bearing_degrees;
|
|
|
54 |
|
|
|
55 |
sprintf(filename,"/dev/i2c-%d");
|
|
|
56 |
if ((file = open(filename,O_RDWR)) < 0) {
|
|
|
57 |
e1 = errno;
|
|
|
58 |
if (e1 != ENOENT) {
|
|
|
59 |
fprintf(stderr,"Error: Could not open file '%s' : %sn",
|
|
|
60 |
filename,strerror(e1));
|
|
|
61 |
if(e1 == EACCES)
|
|
|
62 |
fprintf(stderr,"Run as root?n");
|
|
|
63 |
}
|
|
|
64 |
}
|
|
|
65 |
|
|
|
66 |
if (ioctl(file,I2C_SLAVE,CMPS03_ADDR) < 0) {
|
|
|
67 |
if (errno == EBUSY) {
|
|
|
68 |
printf("device is busyn");
|
|
|
69 |
}
|
|
|
70 |
printf("Got error: %d\n", errno);
|
|
|
71 |
exit(0);
|
|
|
72 |
}
|
|
|
73 |
|
|
|
74 |
/* Get software revision number */
|
|
|
75 |
res = i2c_smbus_read_byte_data(file, CMPS03_SOFTWARE_REVISION);
|
|
|
76 |
if (res < 0) {
|
|
|
77 |
printf("Cannot read software revision leveln");
|
|
|
78 |
} else {
|
|
|
79 |
printf("Software revision level: %02xn", res);
|
|
|
80 |
}
|
|
|
81 |
|
|
|
82 |
/* Loop and read from the compass. */
|
|
|
83 |
while (1) {
|
|
|
84 |
/* The heading byte is 0-255 for the 360 degrees. */
|
|
|
85 |
heading_byte = i2c_smbus_read_byte_data(file, CMPS03_BEARING_BYTE);
|
|
|
86 |
if (heading_byte < 0) { printf("Error reading from compass."); exit(1);}
|
|
|
87 |
|
|
|
88 |
/* The high resolution heading is given in two registers, and is 10 * the
|
|
|
89 |
* heading in degrees, ie 359.9 degrees reads as 3599. */
|
|
|
90 |
heading_word_h = i2c_smbus_read_byte_data(file, CMPS03_BEARING_WORD_HIGH);
|
|
|
91 |
if (heading_word_h < 0) { printf("Error reading from compass."); exit(1);}
|
|
|
92 |
heading_word_l = i2c_smbus_read_byte_data(file, CMPS03_BEARING_WORD_LOW);
|
|
|
93 |
if (heading_word_l < 0) { printf("Error reading from compass."); exit(1);}
|
|
|
94 |
|
|
|
95 |
/* Combine the two bytes, and get the heading in degrees. */
|
|
|
96 |
bearing_long = heading_word_h * 256 + heading_word_l;
|
|
|
97 |
bearing_degrees = bearing_long / 10;
|
|
|
98 |
|
|
|
99 |
printf("Bearing: %d \n", bearing_degrees);
|
|
|
100 |
|
|
|
101 |
/* Wait for a while. */
|
|
|
102 |
usleep(200000);
|
|
|
103 |
}
|
|
|
104 |
}
|