Subversion Repositories svnkaklik

Rev

Rev 358 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 358 Rev 385
Line 1... Line 1...
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
- 
 
 6 
#include <errno.h>
- 
 
 7 
#include <string.h>
- 
 
 8 
#include <stdio.h>
- 
 
 9 
#include <stdlib.h>
5 
 * By Josh Marshall, joshua dot marshall at studentmail dot newcastle dot edu dot au
 10 
#include <unistd.h>
6 
 * October 15, 2004.
 11 
#include <fcntl.h>
- 
 
 12 
#include <linux/i2c-dev.h>
7 
 *
 13 
 
- 
 
 14 
/* Note that the documentation for the compass states its address as 0xC0.
8 
 * Adapted from code for the SP03 speech synthesiser
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
9 
 * By Bram Stolk, b.stolk at chello.nl
 17 
 * address is 0xC0 shifted down, 0x60.
10 
 *
 18 
 */
- 
 
 19 
 
11 
 * Bram Stolk's notes:
 20 
#define CMPS_Addr	(0xC0>>1)
12 
 * In linux, you can do I2C stuff in user-space, if you enable
 21 
#define I2C_SLAVE	0x0B	/* Change slave address
- 
 
 22 
 
13 
 * the device-interface to i2c. You can use plain read() and
23 
/* The important registers on the compass. Internal/test registers omitted. */
- 
 
 24 
#define CMPS03_SOFTWARE_REVISION 0x0
- 
 
 25 
#define CMPS03_BEARING_BYTE 0x1
14 
 * write() calls on your /dev/i2c-0 file.
 26 
#define CMPS03_BEARING_WORD_HIGH 0x2
- 
 
 27 
#define CMPS03_BEARING_WORD_LOW 0x3
- 
 
 28 
#define CMPS03_CALIBRATE_CMD 0xF
15 
 *
 29 
 
16 
 * NOTE: Check if you have an i2c adapter active by doing:
 30 
void I2C_addr (int Addr)  // vybere adresu cidla se kterym se bude komunikovat
17 
 * cat /proc/bus/i2c
 31 
{
18 
 * If this comes up empty, then you have no adapters. If the
 32 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
- 
 
 33 
    {
19 
 * file does not exist at all, you probably lack i2c kernel
 34 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
20 
 * support alltogether.
 35 
        exit(-5);
21 
 */
 36 
    }
- 
 
 37 
}
22 
 
 38 
 
23 
#include <errno.h>
 39 
int i2c_init()   // zinicializuje i2c
- 
 
 40 
{
24 
#include <string.h>
 41 
	file = open("/dev/i2c-0", O_RDWR);
25 
#include <stdio.h>
 42 
	if (file < 0)
- 
 
 43 
	{
26 
#include <stdlib.h>
 44 
		cerr << "Could not open /dev/i2c-0." << endl;
27 
#include <unistd.h>
 45 
		return -1;
- 
 
 46 
	}
28 
#include <fcntl.h>
 47 
	return 0;
- 
 
 48 
}
- 
 
 49 
 
- 
 
 50 
unsigned char read_azimut_mag()  // precte azimut z kompasu
- 
 
 51 
{
29 
#include <linux/i2c-dev.h>
 52 
    char Buf[3];      // promena pro manipulaci s i2c
30 
 
 53 
 
31 
/* Note that the documentation for the compass states its address as 0xC0.
 54 
    I2C_addr(CMPS_Addr);
- 
 
 55 
    Buf[0]=1;
32 
 * However, this includes the low bit which specifies read or write.
 56 
    write(file,Buf,1);
33 
 * Linux i2c does not include this bit in this address, so the actual
 57 
    read(file, Buf,1);
34 
 * address is 0xC0 shifted down, 0x60.
 58 
    return (Buf[0]-SEVER);
35 
 */
 59 
}
36 
#define CMPS03_ADDR	0x60
 - 
 
37 
#define I2C_SLAVE	0x0B	/* Change slave address
- 
 
38 
 
 60 
 
39 
/* The important registers on the compass. Internal/test registers omitted. */
 - 
 
40 
#define CMPS03_SOFTWARE_REVISION 0x0
 61 
void calib()  // kalibrace kompasu
41 
#define CMPS03_BEARING_BYTE 0x1
 - 
 
- 
 
 62 
{
42 
#define CMPS03_BEARING_WORD_HIGH 0x2
 63 
    char Buf[3];      // promena pro manipulaci s i2c
43 
#define CMPS03_BEARING_WORD_LOW 0x3
 - 
 
44 
#define CMPS03_CALIBRATE_CMD 0xF
 - 
 
45 
 
 64 
 
- 
 
 65 
    I2C_addr(CMPS_Addr);
- 
 
 66 
    Buf[0]=15;
- 
 
 67 
    Buf[1]=0xFF;
- 
 
 68 
    write(file,Buf,2);
- 
 
 69 
}
- 
 
 70 
 
46 
int main(int argc, char *argv[]) {
 71 
int main(int argc, char *argv[])
- 
 
 72 
{
47 
   char *end;
 73 
   char *end;
48 
   int res,file;
 74 
   int res,file;
49 
   int error;
 75 
   int error;
50 
   char filename[20] ;
 76 
   char filename[20] ;
51 
   long funcs;
 77 
   long funcs;
52
78
53 
   int heading_byte, heading_word_h, heading_word_l;
 79 
   int heading_byte, heading_word_h, heading_word_l;
54 
   int bearing_long, bearing_degrees;
 80 
   int bearing_long, bearing_degrees;
55
81
56 
   sprintf(filename,"/dev/i2c-0");
 82 
   sprintf(filename,"/dev/i2c-0");
57 
   file = open(filename,O_RDWR);
 83 
   file = open(filename,O_RDWR);
58 
   if (errno != 0)
 84 
   if (errno != 0)
59 
   {
 85 
   {
60 
     switch (errno)
 86 
     switch (errno)
61 
     {
 87 
     {
62 
     case EACCES:
 88 
     case EACCES:
63 
         fprintf(stderr,"Run as root? \n");
 89 
         fprintf(stderr,"Run as root? \n");
64 
         break;
 90 
         break;
65 
     default:
 91 
     default:
66 
         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));
67 
         break;
 93 
         break;
68 
     }
 94 
     }
69 
   }
95 
   }
70
96
71 
   if (ioctl(file,I2C_SLAVE,CMPS03_ADDR) < 0) {
 97 
   if (ioctl(file,I2C_SLAVE,CMPS03_ADDR) < 0) {
72 
     switch (errno)
98 
     switch (errno)
73 
     {
 99 
     {
74 
     case EBUSY:
100 
     case EBUSY:
75 
         printf("device is busy \n");
 101 
         printf("device is busy \n");
76 
         break;
 102 
         break;
77 
 
 103 
 
78 
     default:
 104 
     default:
79 
         printf("Got error: %s \n", strerror(errno));
 105 
         printf("Got error: %s \n", strerror(errno));
80 
         exit(0);
 106 
         exit(0);
81 
      }
 107 
      }
82 
   }
 108 
   }
83
109
84 
   /* Get software revision number */
 110 
   /* Get software revision number */
85 
   res = read(file, CMPS03_SOFTWARE_REVISION,1);
 111 
   res = read(file, CMPS03_SOFTWARE_REVISION,1);
86 
   if (res < 0) {
 112 
   if (res < 0) {
87 
     printf("Cannot read software revision level \n");
 113 
     printf("Cannot read software revision level \n");
88 
   } else {
 114 
   } else {
89 
     printf("Software revision level: %02x \n", res);
 115 
     printf("Software revision level: %02x \n", res);
90 
   }
 116 
   }
91
117
92 
   /* Loop and read from the compass. */
 118 
   /* Loop and read from the compass. */
93 
   while (1) {
 119 
   while (1) {
94 
     /* The heading byte is 0-255 for the 360 degrees. */
 120 
     /* The heading byte is 0-255 for the 360 degrees. */
95 
     heading_byte = read(file, CMPS03_BEARING_BYTE,1);
 121 
     heading_byte = read(file, CMPS03_BEARING_BYTE,1);
96 
     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);}
97
123
98 
     /* 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
99 
      * heading in degrees, ie 359.9 degrees reads as 3599. */
 125 
      * heading in degrees, ie 359.9 degrees reads as 3599. */
100 
     heading_word_h = read(file, CMPS03_BEARING_WORD_HIGH,1);
 126 
     heading_word_h = read(file, CMPS03_BEARING_WORD_HIGH,1);
101 
     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);}
102 
     heading_word_l = read(file, CMPS03_BEARING_WORD_LOW,1);
 128 
     heading_word_l = read(file, CMPS03_BEARING_WORD_LOW,1);
103 
     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);}
104
130
105 
     /* Combine the two bytes, and get the heading in degrees. */
 131 
     /* Combine the two bytes, and get the heading in degrees. */
106 
     bearing_long = heading_word_h * 256 + heading_word_l;
 132 
     bearing_long = heading_word_h * 256 + heading_word_l;
107 
     bearing_degrees = bearing_long / 10;
 133 
     bearing_degrees = bearing_long / 10;
108
134
109 
     printf("Bearing: %d \n", bearing_degrees);
 135 
     printf("Bearing: %d \n", bearing_degrees);
110
136
111 
     /* Wait for a while. */
 137 
     /* Wait for a while. */
112 
     usleep(200000);
 138 
     usleep(200000);
113 
   }
 139 
   }
114 
}
 140 
}