From 79e5a3b1a806faf6ff2b1b24d496e6ddf776461f Mon Sep 17 00:00:00 2001
From: Patrick Goebel <patrick@pirobot.org>
Date: Mon, 11 Apr 2016 08:03:37 -0700
Subject: [PATCH] Updated imu.h and imu.ino and increased command argv array
 size from 16 to 32 to handel PID update

---
 .../ROSArduinoBridge/ROSArduinoBridge.ino     | 14 ++++++++-----
 .../src/libraries/ROSArduinoBridge/imu.h      | 12 ++++++++++-
 .../src/libraries/ROSArduinoBridge/imu.ino    | 21 ++++++++++---------
 3 files changed, 31 insertions(+), 16 deletions(-)

diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
index 839fa8e..a8e7201 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
@@ -143,6 +143,7 @@
 #ifdef USE_IMU
   #include "imu.h"
 
+  // The only IMU currently supported is the Adafruit 9-DOF IMU
   #define ADAFRUIT_9DOF
 
 #endif
@@ -161,8 +162,8 @@ char chr;
 char cmd;
 
 // Character arrays to hold the first and second arguments
-char argv1[16];
-char argv2[16];
+char argv1[32];
+char argv2[32];
 
 // The arguments converted to integers
 long arg1;
@@ -220,6 +221,9 @@ int runCommand() {
 #ifdef USE_IMU
   case READ_IMU:
     imu_data = readIMU();
+    /* Send the IMU data base in the following order
+     * [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch]
+     */
     Serial.print(imu_data.ax);
     Serial.print(F(" "));
     Serial.print(imu_data.ay);
@@ -242,7 +246,7 @@ int runCommand() {
     Serial.print(F(" "));
     Serial.print(imu_data.pitch);
     Serial.print(F(" "));
-    Serial.println(imu_data.uh);
+    Serial.println(imu_data.ch);
     break;
 #endif
 #ifdef USE_SERVOS
@@ -322,8 +326,8 @@ int runCommand() {
   case UPDATE_PID:
     i = 0;
     while ((str = strtok_r(p, ":", &p)) != '\0') {
-       pid_args[i] = atoi(str);
-       i++;
+      pid_args[i] = atoi(str);
+      i++;
     }
     Kp = pid_args[0];
     Kd = pid_args[1];
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h
index 64c5f9f..e1fc077 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h
@@ -3,6 +3,16 @@
 #ifndef IMU_H
 #define IMU_H
 
+/*
+  IMU data is assumed to be returned in the following order:
+    
+  [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch]
+    
+  where a stands for accelerometer, g for gyroscope and m for magnetometer.
+  The last value ch stands for "compensated heading" that some IMU's can 
+  compute to compensate magnetic heading from the current roll and pitch.
+*/
+
 typedef struct imuData_s
     {
       float ax = -999;
@@ -16,7 +26,7 @@ typedef struct imuData_s
       float mz = -999;
       float roll = -999;
       float pitch = -999;
-      float uh = -999;
+      float ch = -999;
     } imuData;
 
     void initIMU();
diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
index aefbecc..13bd811 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
@@ -12,22 +12,17 @@
     Adafruit_9DOF                 dof   = Adafruit_9DOF();
     Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
     Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
+    Adafruit_L3GD20_Unified       gyro  = Adafruit_L3GD20_Unified(20);
+
     
     /* Update this with the correct SLP for accurate altitude measurements */
     float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
     
     void initIMU()
     {
-      if(!accel.begin())
-      {
-        /* There was a problem detecting the LSM303 ... check your connections */
-        Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
-      }
-      if(!mag.begin())
-      {
-        /* There was a problem detecting the LSM303 ... check your connections */
-        Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
-      }
+      accel.begin();
+      mag.begin();
+      gyro.begin();
     }
   
     imuData readIMU() {
@@ -36,6 +31,7 @@
       sensors_event_t accel_event;
       sensors_event_t mag_event;
       sensors_vec_t   orientation;
+      sensors_event_t event;
       
       /* Calculate pitch and roll from the raw accelerometer data */
       accel.getEvent(&accel_event);
@@ -43,6 +39,11 @@
       data.ay = accel_event.acceleration.y;
       data.az = accel_event.acceleration.z;
 
+      gyro.getEvent(&event);
+      data.gx = event.gyro.x;
+      data.gy = event.gyro.y;
+      data.gz = event.gyro.z;
+
       if (dof.accelGetOrientation(&accel_event, &orientation))
       {
          /* 'orientation' should have valid .roll and .pitch fields */