Diff: Sensors.cpp

Revision:

1:e27c4c0b71d8

Parent:

0:9a72d42c0da3

--- a/Sensors.cpp	Tue Dec 27 17:20:06 2011 +0000
+++ b/Sensors.cpp	Wed Dec 28 17:13:14 2011 +0000
@@ -6,161 +6,147 @@
 // Sensor I2C addresses
 #define ACCEL_READ  0xA7
 #define ACCEL_WRITE 0xA6
-#define MAGN_WRITE  0x3C 
-#define MAGN_READ   0x3D 
+#define MAGN_WRITE  0x3C
+#define MAGN_READ   0x3D
 #define GYRO_WRITE  0xD0
 #define GYRO_READ   0xD1
 
-void IMU::I2C_Init()
-{
-  Wire.frequency(100000);
+void IMU::I2C_Init() {
+    Wire.frequency(100000);
 }
 
-void IMU::Accel_Init()
-{
-  char tx[2];
-  tx[0] = 0x2D; // Power register
-  tx[1] = 0x08; // Power register
-  Wire.write(ACCEL_WRITE, tx, 2);
-  wait_ms(5);
-  tx[0] = 0x31; // Data format register
-  tx[1] = 0x08; // Set to full resolution
-  Wire.write(ACCEL_WRITE, tx, 2);
-  wait_ms(5);
-  
-  // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
-  tx[0] = 0x2C;  // Rate
-  tx[1] = 0x09;  // Set to 50Hz, normal operation
-  Wire.write(ACCEL_WRITE, tx, 2);
-  wait_ms(5);
+void IMU::Accel_Init() {
+    char tx[2];
+
+    tx[0] = 0x2D; // Power register
+    tx[1] = 0x08; // Power register
+    Wire.write(ACCEL_WRITE, tx, 2);
+    wait_ms(5);
+
+    tx[0] = 0x31; // Data format register
+    tx[1] = 0x08; // Set to full resolution
+    Wire.write(ACCEL_WRITE, tx, 2);
+    wait_ms(5);
+
+    // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
+    tx[0] = 0x2C;  // Rate
+    tx[1] = 0x09;  // Set to 50Hz, normal operation
+    Wire.write(ACCEL_WRITE, tx, 2);
+    wait_ms(5);
 }
 
 // Reads x, y and z accelerometer registers
-void IMU::Read_Accel()
-{
-  char buff[6];
-  char tx = 0x32;  // Send address to read from
- 
-  Wire.write(ACCEL_WRITE, &tx, 1);
-  
-  if (Wire.read(ACCEL_READ, buff, 6) == 0) // All bytes received?
-  {
-    // No multiply by -1 for coordinate system transformation here, because of double negation:
-    // We want the gravity vector, which is negated acceleration vector.
-    accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis)
-    accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis)
-    accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis)
-  }
-  else
-  {
-    num_accel_errors++;
-    if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
-  }
+void IMU::Read_Accel() {
+    char buff[6];
+    char tx = 0x32;  // Send address to read from
+
+    Wire.write(ACCEL_WRITE, &tx, 1);
+
+    if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received?
+        // No multiply by -1 for coordinate system transformation here, because of double negation:
+        // We want the gravity vector, which is negated acceleration vector.
+        accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis)
+        accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis)
+        accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis)
+    } else {
+        num_accel_errors++;
+        if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
+    }
 }
 
-void IMU::Magn_Init()
-{
-  char tx[2];
-  tx[0] = 0x02; // Mode
-  tx[1] = 0x00; // Set continuous mode (default 10Hz)
-  Wire.write(MAGN_WRITE, tx, 2);
-  wait_ms(5);
+void IMU::Magn_Init() {
+    char tx[2];
+    tx[0] = 0x02; // Mode
+    tx[1] = 0x00; // Set continuous mode (default 10Hz)
+    Wire.write(MAGN_WRITE, tx, 2);
+    wait_ms(5);
 
-  tx[0] = 0x00; // CONFIG_A 
-  tx[1] = 0x18; // Set 50Hz
-  Wire.write(MAGN_WRITE, tx, 2);
-  wait_ms(5);
+    tx[0] = 0x00; // CONFIG_A
+    tx[1] = 0x18; // Set 50Hz
+    Wire.write(MAGN_WRITE, tx, 2);
+    wait_ms(5);
 }
 
-void IMU::Read_Magn()
-{
-  char buff[6];
-  char tx = 0x03;  // Send address to read from 
+void IMU::Read_Magn() {
+    char buff[6];
+    char tx = 0x03;  // Send address to read from
 
-  Wire.write(MAGN_WRITE, &tx, 1);
-  
-  if (Wire.read(MAGN_READ, buff, 6) == 0)  // All bytes received?
-  {
+    Wire.write(MAGN_WRITE, &tx, 1);
+
+    if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received?
 // 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
 #if HW__VERSION_CODE == 10125
-    // MSB byte first, then LSB; X, Y, Z
-    magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3];  // X axis (internal sensor -y axis)
-    magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
-    magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
+        // MSB byte first, then LSB; X, Y, Z
+        magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3];  // X axis (internal sensor -y axis)
+        magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
+        magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
 // 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
 #elif HW__VERSION_CODE == 10736
-    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
-    magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5];  // X axis (internal sensor -y axis)
-    magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
-    magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3];  // Z axis (internal sensor -z axis)
+        // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
+        magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5];  // X axis (internal sensor -y axis)
+        magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1];  // Y axis (internal sensor -x axis)
+        magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3];  // Z axis (internal sensor -z axis)
 // 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
 #elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
-    // MSB byte first, then LSB; X, Y, Z
-    magnetom[0] = (((int) buff[0]) << 8) | buff[1];       // X axis (internal sensor x axis)
-    magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3];  // Y axis (internal sensor -y axis)
-    magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
+        // MSB byte first, then LSB; X, Y, Z
+        magnetom[0] = (((int) buff[0]) << 8) | buff[1];       // X axis (internal sensor x axis)
+        magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3];  // Y axis (internal sensor -y axis)
+        magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5];  // Z axis (internal sensor -z axis)
 // 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
 #elif HW__VERSION_CODE == 10724
-    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
-    magnetom[0] =  1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis)
-    magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis)
-    magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis)
+        // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
+        magnetom[0] =  1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis)
+        magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis)
+        magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis)
 #endif
-  }
-  else
-  {
-    num_magn_errors++;
-    if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
-  }
+    } else {
+        num_magn_errors++;
+        if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
+    }
 }
 
-void IMU::Gyro_Init()
-{
-  char tx[2];
-  
-  // Power up reset defaults
-  tx[0] = 0x3E; // Power management
-  tx[1] = 0x80; // ?
-  Wire.write(GYRO_WRITE, tx, 2);
-  wait_ms(5);
-  
-  // Select full-scale range of the gyro sensors
-  // Set LP filter bandwidth to 42Hz
-  tx[0] = 0x16; // 
-  tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
-  Wire.write(GYRO_WRITE, tx, 2);
-  wait_ms(5);
-  
-  // Set sample rato to 50Hz
-  tx[0] = 0x15; //
-  tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
-  Wire.write(GYRO_WRITE, tx, 2);
-  wait_ms(5);
+void IMU::Gyro_Init() {
+    char tx[2];
+
+    // Power up reset defaults
+    tx[0] = 0x3E; // Power management
+    tx[1] = 0x80; // ?
+    Wire.write(GYRO_WRITE, tx, 2);
+    wait_ms(5);
 
-  // Set clock to PLL with z gyro reference
-  tx[0] = 0x3E;
-  tx[1] = 0x00;
-  Wire.write(GYRO_WRITE, tx, 2);
-  wait_ms(5);
+    // Select full-scale range of the gyro sensors
+    // Set LP filter bandwidth to 42Hz
+    tx[0] = 0x16; //
+    tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
+    Wire.write(GYRO_WRITE, tx, 2);
+    wait_ms(5);
+
+    // Set sample rato to 50Hz
+    tx[0] = 0x15; //
+    tx[1] = 0x0A; //  SMPLRT_DIV = 10 (50Hz)
+    Wire.write(GYRO_WRITE, tx, 2);
+    wait_ms(5);
+
+    // Set clock to PLL with z gyro reference
+    tx[0] = 0x3E;
+    tx[1] = 0x00;
+    Wire.write(GYRO_WRITE, tx, 2);
+    wait_ms(5);
 }
 
 // Reads x, y and z gyroscope registers
-void IMU::Read_Gyro()
-{
-  char buff[6];
-  char tx = 0x1D; // Sends address to read from
-  
-  Wire.write(GYRO_WRITE, &tx, 1);
-  
-  if (Wire.read(GYRO_READ, buff, 6) == 0)  // All bytes received?
-  {
-    gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]);   // X axis (internal sensor -y axis)
-    gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]);   // Y axis (internal sensor -x axis)
-    gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]);   // Z axis (internal sensor -z axis)
-  }
-  else
-  {
-    num_gyro_errors++;
-    if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
-  }
+void IMU::Read_Gyro() {
+    char buff[6];
+    char tx = 0x1D; // Sends address to read from
+
+    Wire.write(GYRO_WRITE, &tx, 1);
+
+    if (Wire.read(GYRO_READ, buff, 6) == 0) { // All bytes received?
+        gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]);   // X axis (internal sensor -y axis)
+        gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]);   // Y axis (internal sensor -x axis)
+        gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]);   // Z axis (internal sensor -z axis)
+    } else {
+        num_gyro_errors++;
+        if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
+    }
 }