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
}