Added non-final versions of iris and tower firmware.

software/firmware/tower/tower.ino

@@ -1,9 +1,159 @@
+#include <NMEAGPS.h>
+#include <ublox/ubxGPS.h>
+#include <NeoGPS_cfg.h>
+#include <Adafruit_BNO055_t3.h>
+
+
+/*
+   Imu/data (Imu)
+   Imu/raw (Imu)
+   Imu/mag (MagneticField)
+   Imu/temp (Temperature)
+
+   ---Raw Data (Imu/raw) ---
+   LinearAccel (x, y, z)
+   AngularVel (x, y, z);
+
+   --- Filtered Data (Imu/data) ---
+   Orientation(z, y, z, w)
+   LinearAccel(x, y, z)
+   AngularVel(x, y, z)
+
+   --- Mag Data (Imu/mag) ---
+   MagField(x, y, z)
+
+   --- IMU Temp (Imu/temp)
+   temp (deg c)
+*/
+
+Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR);
+
+
+#define gpsPort Serial3
+#define commsPort Serial2
+#define GPS_PORT_NAME "Serial3"
+
+const unsigned char ubxRate5Hz[] = { 0x06, 0x08, 0x06, 0x00, 200, 0x00, 0x01, 0x00, 0x01, 0x00 };
+const unsigned char ubxRate10Hz[] = { 0x06, 0x08, 0x06, 0x00, 100, 0x00, 0x01, 0x00, 0x01, 0x00 };
+
+const char baud38400 [] = "PUBX,41,1,3,3,38400,0";
+const char baud57600 [] = "PUBX,41,1,3,3,57600,0";
+const char baud115200[] = "PUBX,41,1,3,3,115200,0";
+
+imu::Vector<3> linear_accel;
+imu::Vector<3>  angular_vel;
+imu::Quaternion quat;
+
+char current_byte = '$';
+String nmea_sentence = "";
+String output_sentence;
+
+char float_decimal_places = 8;
+
+void sendUBX( const unsigned char *progmemBytes, size_t len )
+{
+  gpsPort.write( 0xB5 ); // SYNC1
+  gpsPort.write( 0x62 ); // SYNC2
+
+  uint8_t a = 0, b = 0;
+  while (len-- > 0) {
+    uint8_t c = pgm_read_byte( progmemBytes++ );
+    a += c;
+    b += a;
+    gpsPort.write( c );
+  }
+
+  gpsPort.write( a ); // CHECKSUM A
+  gpsPort.write( b ); // CHECKSUM B
+
+}
+
 void setup() {
   // put your setup code here, to run once:
+  commsPort.begin(115200);
+  commsPort.transmitterEnable(6);
+
+  delay(250);
+  gpsPort.begin(9600);
+  sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) );
+
+  if (!bno.begin()) {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    commsPort.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while (1);
+  }
+  bno.setExtCrystalUse(true);
+
+
 
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+
+  if (gpsPort.available() > 0 ) {
+    nmea_sentence = "";
+    do {
+      if (gpsPort.available() > 0) {
+        current_byte = gpsPort.read();
+
+        if (current_byte != '\r' and current_byte != '\n') {
+          nmea_sentence += current_byte;
+        }
+      }
+    } while (current_byte != '\r');
+    commsPort.println(nmea_sentence);
+  }
+
+  quat = bno.getQuat();
+  linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+  angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
+
+  commsPort.print("ox:");
+  commsPort.print(quat.x(), float_decimal_places);
+  commsPort.print(",");
+  commsPort.print("oy:");
+  commsPort.print(quat.y(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("oz:");
+  commsPort.print(quat.z(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("ow:");
+  commsPort.print(quat.w(), float_decimal_places);
+
+    commsPort.print(",");
+  commsPort.print("lax:");
+  commsPort.print(linear_accel.x(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("lay:");
+  commsPort.print(linear_accel.y(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("laz:");
+  commsPort.print(linear_accel.z(), float_decimal_places);
+
+  
+    commsPort.print(",");
+  commsPort.print("avx:");
+  commsPort.print(angular_vel.x(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("avy:");
+  commsPort.print(angular_vel.y(), float_decimal_places);
+    commsPort.print(",");
+  commsPort.print("avz:");
+  commsPort.print(angular_vel.z(), float_decimal_places);
+  
+  commsPort.println();
+
+  //
+  //  /* Display calibration status for each sensor. */
+  //  uint8_t system, gyro, accel, mag = 0;
+  //  bno.getCalibration(&system, &gyro, &accel, &mag);
+  //  commsPort.print("CALIBRATION: Sys=");
+  //  commsPort.print(system, DEC);
+  //  commsPort.print(" Gyro=");
+  //  commsPort.print(gyro, DEC);
+  //  commsPort.print(" Accel=");
+  //  commsPort.print(accel, DEC);
+  //  commsPort.print(" Mag=");
+  //  commsPort.print(mag, DEC);
 
 }