Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

quad/src/virt_quad/hw_impl_unix_i2c.c

@@ -5,6 +5,8 @@
 #include <pthread.h>
 #include "iic_utils.h"
 
+#define NUM_INPUTS 7
+
 void * update_i2c_input_cache(void *);
 
 union val {
@@ -12,16 +14,16 @@ union val {
   unsigned short s;
 };
 
-static char *input_names[6];
-static int fifos[6];
-static union val cache[6];
+static char *input_names[NUM_INPUTS];
+static int fifos[NUM_INPUTS];
+static union val cache[NUM_INPUTS];
 
 static short last_dev;
 static short last_reg;
 static short last_val;
 
 static int nums[] = {0, 1, 2, 3, 4, 5};
-static pthread_t workers[6];
+static pthread_t workers[NUM_INPUTS];
 
 int unix_i2c_reset(struct I2CDriver *self) {
   input_names[0] = "i2c-mpu-accel-x";
@@ -30,12 +32,13 @@ int unix_i2c_reset(struct I2CDriver *self) {
   input_names[3] = "i2c-mpu-gryo-x";
   input_names[4] = "i2c-mpu-gryo-y";
   input_names[5] = "i2c-mpu-gyro-z";
+  input_names[6] = "i2c-lidar";
 
   mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
 
   // Start up worker thread whose job is to update the caches
   int i;
-  for (i = 0; i < 6; i += 1) {
+  for (i = 0; i < NUM_INPUTS; i += 1) {
     pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
   }
 
@@ -45,6 +48,7 @@ int unix_i2c_reset(struct I2CDriver *self) {
   cache[3].s = 0;
   cache[4].s = 0;
   cache[5].s = 0;
+  cache[6].s = 0;
 
   return 0;
 }
@@ -87,6 +91,13 @@ int unix_i2c_read(struct I2CDriver *self,
       buff[12] = cache[5].b[0];
       buff[13] = cache[5].b[1];
     }
+    else if (last_reg == LIDARLITE_DEVICE_ADDR) {
+      buff[0] = cache[6].b[0];
+      buff[1] = cache[6].b[0];
+    }
+    else {
+      return -1;
+    }
   }
   return 0;
 }