Mateusz Jaskula
/
KL25Z_TangoIO
KL25 driver for Tango Control System
GY80/GY80.h@2:9fe6f1e273b4, 2014-08-28 (annotated)
- Committer:
- jskl
- Date:
- Thu Aug 28 07:50:06 2014 +0000
- Revision:
- 2:9fe6f1e273b4
- Parent:
- 0:5d27c333afa6
Fixed bugs in JSON format replies
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jskl | 0:5d27c333afa6 | 1 | #ifndef SENSORS_H |
jskl | 0:5d27c333afa6 | 2 | #define SENSORS_H |
jskl | 0:5d27c333afa6 | 3 | |
jskl | 0:5d27c333afa6 | 4 | #include "mbed.h" |
jskl | 0:5d27c333afa6 | 5 | #define I2C_FREQ 400000 |
jskl | 0:5d27c333afa6 | 6 | #define SCL PTC8 |
jskl | 0:5d27c333afa6 | 7 | #define SDA PTC9 |
jskl | 0:5d27c333afa6 | 8 | |
jskl | 0:5d27c333afa6 | 9 | #define ACCEL_ADDRESS (0xA6) // 0x53 = 0xA6 / 2 |
jskl | 0:5d27c333afa6 | 10 | #define MAGN_ADDRESS (0x3C) // 0x1E = 0x3C / 2 |
jskl | 0:5d27c333afa6 | 11 | #define GYRO_ADDRESS (0xD2) // 0x68 = 0xD0 / 2 |
jskl | 0:5d27c333afa6 | 12 | |
jskl | 0:5d27c333afa6 | 13 | // SENSOR CALIBRATION |
jskl | 0:5d27c333afa6 | 14 | /*****************************************************************/ |
jskl | 0:5d27c333afa6 | 15 | // How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs |
jskl | 0:5d27c333afa6 | 16 | // Put MIN/MAX and OFFSET readings for your board here! |
jskl | 0:5d27c333afa6 | 17 | // Accelerometer |
jskl | 0:5d27c333afa6 | 18 | // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" |
jskl | 0:5d27c333afa6 | 19 | #define ACCEL_X_MIN (-255.0f) |
jskl | 0:5d27c333afa6 | 20 | #define ACCEL_X_MAX (260.0f) |
jskl | 0:5d27c333afa6 | 21 | #define ACCEL_Y_MIN (-267.0f) |
jskl | 0:5d27c333afa6 | 22 | #define ACCEL_Y_MAX (255.0f) |
jskl | 0:5d27c333afa6 | 23 | #define ACCEL_Z_MIN (-278.0f) |
jskl | 0:5d27c333afa6 | 24 | #define ACCEL_Z_MAX (238.0f) |
jskl | 0:5d27c333afa6 | 25 | |
jskl | 0:5d27c333afa6 | 26 | // Magnetometer |
jskl | 0:5d27c333afa6 | 27 | // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" |
jskl | 0:5d27c333afa6 | 28 | #define MAGN_X_MIN (-520.0f) |
jskl | 0:5d27c333afa6 | 29 | #define MAGN_X_MAX (850.0f) |
jskl | 0:5d27c333afa6 | 30 | #define MAGN_Y_MIN (-640.0f) |
jskl | 0:5d27c333afa6 | 31 | #define MAGN_Y_MAX (684.0f) |
jskl | 0:5d27c333afa6 | 32 | #define MAGN_Z_MIN (-213.0f) |
jskl | 0:5d27c333afa6 | 33 | #define MAGN_Z_MAX (850.0f) |
jskl | 0:5d27c333afa6 | 34 | |
jskl | 0:5d27c333afa6 | 35 | // Gyroscope |
jskl | 0:5d27c333afa6 | 36 | // "gyro x,y,z (current/average) = .../OFFSET_X .../OFFSET_Y .../OFFSET_Z |
jskl | 0:5d27c333afa6 | 37 | #define GYRO_X_OFFSET (-20.5f) |
jskl | 0:5d27c333afa6 | 38 | #define GYRO_Y_OFFSET (0.5f) |
jskl | 0:5d27c333afa6 | 39 | #define GYRO_Z_OFFSET (10.5f) |
jskl | 0:5d27c333afa6 | 40 | |
jskl | 0:5d27c333afa6 | 41 | //*****************************************************************************/ |
jskl | 0:5d27c333afa6 | 42 | |
jskl | 0:5d27c333afa6 | 43 | #define GRAVITY 255.0f //this equivalent to 1G in the raw data coming from the accelerometer |
jskl | 0:5d27c333afa6 | 44 | |
jskl | 0:5d27c333afa6 | 45 | #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 46 | #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 47 | #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 48 | #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) |
jskl | 0:5d27c333afa6 | 49 | #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)) |
jskl | 0:5d27c333afa6 | 50 | #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET)) |
jskl | 0:5d27c333afa6 | 51 | |
jskl | 0:5d27c333afa6 | 52 | #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 53 | #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 54 | #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) |
jskl | 0:5d27c333afa6 | 55 | #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)) |
jskl | 0:5d27c333afa6 | 56 | #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)) |
jskl | 0:5d27c333afa6 | 57 | #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)) |
jskl | 0:5d27c333afa6 | 58 | |
jskl | 0:5d27c333afa6 | 59 | // Gyro gain (conversion from raw to degree per seconds) |
jskl | 0:5d27c333afa6 | 60 | #define GYRO_GAIN 0.061035156f |
jskl | 0:5d27c333afa6 | 61 | #define GYRO_GAIN_X 0.061035156f //X axis Gyro gain |
jskl | 0:5d27c333afa6 | 62 | #define GYRO_GAIN_Y 0.061035156f //Y axis Gyro gain |
jskl | 0:5d27c333afa6 | 63 | #define GYRO_GAIN_Z 0.061035156f //Z axis Gyro gain |
jskl | 0:5d27c333afa6 | 64 | |
jskl | 0:5d27c333afa6 | 65 | #define DEG2RAD(x) (x * 0.01745329252) // *pi/180 |
jskl | 0:5d27c333afa6 | 66 | #define RAD2DEG(x) (x * 57.2957795131) // *180/pi |
jskl | 0:5d27c333afa6 | 67 | |
jskl | 0:5d27c333afa6 | 68 | typedef char byte; |
jskl | 0:5d27c333afa6 | 69 | |
jskl | 0:5d27c333afa6 | 70 | class GY80 |
jskl | 0:5d27c333afa6 | 71 | { |
jskl | 0:5d27c333afa6 | 72 | public: |
jskl | 0:5d27c333afa6 | 73 | I2C Wire; |
jskl | 0:5d27c333afa6 | 74 | GY80(); |
jskl | 0:5d27c333afa6 | 75 | ~GY80(); |
jskl | 0:5d27c333afa6 | 76 | void Read_Accel(float* accel_v); |
jskl | 0:5d27c333afa6 | 77 | void Read_Gyro(float* ); |
jskl | 0:5d27c333afa6 | 78 | void Read_Magn(float* ); |
jskl | 0:5d27c333afa6 | 79 | |
jskl | 0:5d27c333afa6 | 80 | private: |
jskl | 0:5d27c333afa6 | 81 | short accel[3]; |
jskl | 0:5d27c333afa6 | 82 | short gyro[3]; |
jskl | 0:5d27c333afa6 | 83 | short mag[3]; |
jskl | 0:5d27c333afa6 | 84 | void Accel_Init(); |
jskl | 0:5d27c333afa6 | 85 | void Gyro_Init(); |
jskl | 0:5d27c333afa6 | 86 | void Magn_Init(); |
jskl | 0:5d27c333afa6 | 87 | |
jskl | 0:5d27c333afa6 | 88 | }; |
jskl | 0:5d27c333afa6 | 89 | |
jskl | 0:5d27c333afa6 | 90 | #endif |