From 7c42e69c7bdacda4c07e334eaae6efe3e74d2a8d Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Fri, 7 Apr 2017 20:32:27 -0500
Subject: [PATCH] quad: add lidar to virtual quad inputs
---
quad/src/virt_quad/hw_impl_unix_i2c.c | 21 ++++++++++++++++-----
1 file changed, 16 insertions(+), 5 deletions(-)
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index b77e635a5..97228f0ba 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/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;
}
--
GitLab