| 0,0 → 1,140 |
|---|
| /* 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); |
| } |
| } |