quad: create tests script for virtual quad

And add I2C implementation for virtual quad

quad/scripts/tests/run_virtual_test_flight.rb

+#!/usr/bin/env ruby
+
+# Test Flight
+#
+# A simple virtual test flight (take off, hover, and set back down)
+#
+
+THROTTLE_MIN = 110200
+THROTTLE_MAX = 191900
+THROTTLE_MID = (THROTTLE_MAX + THROTTLE_MIN)/2
+THROTTLE_3_4 = (THROTTLE_MAX + THROTTLE_MID)/2
+THROTTLE_QUAR = (THROTTLE_MID + THROTTLE_MIN)/2
+THROTTLE_EIGHTH = (THROTTLE_QUAR + THROTTLE_MIN)/2
+THROTTLE_16 = (THROTTLE_EIGHTH + THROTTLE_MIN)/2
+MOTOR_MIN = 100000
+MOTOR_MAX = 200000
+GEAR_ON = 170800
+GEAR_OFF = 118300
+GRAVITY = 4096
+
+MOTOR1 = "virt-quad-fifos/pwm-output-motor1"
+MOTOR2 = "virt-quad-fifos/pwm-output-motor2"
+MOTOR3 = "virt-quad-fifos/pwm-output-motor3"
+MOTOR4 = "virt-quad-fifos/pwm-output-motor4"
+
+UART_TX = "virt-quad-fifos/uart-tx"
+
+GEAR = "virt-quad-fifos/pwm-input-gear"
+THROTTLE = "virt-quad-fifos/pwm-input-throttle"
+
+LED = "virt-quad-fifos/mio7-led"
+
+I2C_MPU_ACCEL_X = "virt-quad-fifos/i2c-mpu-accel-x"
+I2C_MPU_ACCEL_Y = "virt-quad-fifos/i2c-mpu-accel-y"
+I2C_MPU_ACCEL_Z = "virt-quad-fifos/i2c-mpu-accel-z"
+
+require 'test/unit/assertions'
+require 'thread'
+include Test::Unit::Assertions
+
+script_dir = File.expand_path(File.dirname(__FILE__))
+bin_dir = script_dir + "/../../bin/"
+Dir.chdir(bin_dir)
+
+# Start virtual quad
+quad = Process.spawn("./virt-quad")
+
+sleep 1
+
+##################
+#  Begin Flight!
+##################
+
+begin
+  puts("Starting flight")
+
+  # Set gravity
+  File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
+
+  puts("Turning on GEAR...")
+  File.write(GEAR, GEAR_ON)
+  sleep 0.015
+
+  puts("Increasing Thrust to half maximum...")
+  for i in (THROTTLE_MIN..THROTTLE_MID).step(1000)
+    File.write(THROTTLE, i)
+    sleep 0.005
+  end
+
+  puts("Hovering for 10 seconds")
+  sleep 10
+
+  puts("Relaxing thrust to zero")
+  i = THROTTLE_MID
+  while i > THROTTLE_MIN
+    i -= 1000
+    File.write(THROTTLE, i)
+    sleep 0.005
+  end
+
+  puts("Swiching off GEAR...")
+  File.write(GEAR, GEAR_OFF)
+
+  # puts("Collecting logs...")
+  # msg = ""
+  # misses = 0
+  # fifo = File::open(UART_TX)
+  # while misses < 10
+  #   puts "trying..."
+  #   if fifo.eof?
+  #     misses += 1
+  #     next
+  #   end
+  #   msg += fifo.read()
+  # end
+  
+  # fifo.close()
+
+  # puts msg
+
+ensure
+
+  Process.kill(9, quad)
+
+end
+

quad/scripts/tests/test_safety_checks.rb

@@ -15,6 +15,7 @@ MOTOR_MIN = 100000
 MOTOR_MAX = 200000
 GEAR_ON = 170800
 GEAR_OFF = 118300
+GRAVITY = 4096
 
 MOTOR1 = "virt-quad-fifos/pwm-output-motor1"
 MOTOR2 = "virt-quad-fifos/pwm-output-motor2"
@@ -26,12 +27,21 @@ THROTTLE = "virt-quad-fifos/pwm-input-throttle"
 
 LED = "virt-quad-fifos/mio7-led"
 
+I2C_MPU_ACCEL_X = "virt-quad-fifos/i2c-mpu-accel-x"
+I2C_MPU_ACCEL_Y = "virt-quad-fifos/i2c-mpu-accel-y"
+I2C_MPU_ACCEL_Z = "virt-quad-fifos/i2c-mpu-accel-z"
+
 require 'test/unit/assertions'
 require 'thread'
 include Test::Unit::Assertions
 
-def read_fifo_num(fifo)
-  File.read(fifo).chomp.split("\n").last.to_i
+$fifos = Hash.new
+
+def read_fifo_num(f)
+  if not $fifos.key?(f)
+    $fifos[f] = File.open(f)
+  end
+  $fifos[f].read().chomp.split("\n").last.to_i
 end
 
 # Utility functions
@@ -46,6 +56,23 @@ def check_motors_are_off
   assert_operator motor4, :<=, THROTTLE_MIN
 end
 
+def get_motor_averages
+  motors = [[], [], [], []]
+  for i in 0..100
+    motors[0].push(read_fifo_num(MOTOR1))
+    motors[1].push(read_fifo_num(MOTOR2))
+    motors[2].push(read_fifo_num(MOTOR3))
+    motors[3].push(read_fifo_num(MOTOR4))
+    sleep 0.010
+  end
+  average = []
+  average[0] = motors[0].inject(:+).to_f / motors[0].size
+  average[1] = motors[1].inject(:+).to_f / motors[1].size
+  average[2] = motors[2].inject(:+).to_f / motors[2].size
+  average[3] = motors[3].inject(:+).to_f / motors[3].size
+  average
+end
+
 def check_led(on)
   led = read_fifo_num(LED)
   assert_equal(led, on)
@@ -65,14 +92,18 @@ sleep 1
 #################
 
 begin
+  puts("beginning tests")
+  
+  # Set gravity
+  File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
 
-  # Check that motors are off at startup
+  puts("Check that motors are off at startup")
   check_motors_are_off
 
-  # Check that LED is off at startup
+  puts("Check that LED is off at startup")
   check_led(0)
 
-  # Check that increasing the throttle does nothing to motors
+  puts("Check that increasing the throttle does nothing to motors")
   # (because gear is still off)
   for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
     File.write(THROTTLE, i)
@@ -80,7 +111,7 @@ begin
     sleep 0.005
   end
 
-  # Check that flipping gear to 1 while throttle is high does nothing
+  puts("Check that flipping gear to 1 while throttle is high does nothing")
   # (motors should still be off, LED should still be off)
   File.write(GEAR, GEAR_ON)
   sleep 0.015
@@ -98,38 +129,24 @@ begin
   File.write(GEAR, GEAR_OFF)
   File.write(THROTTLE, THROTTLE_MIN)
 
-  # Check that the LED turns on when gear is flipped on
+  puts("Check that the LED turns on when gear is flipped on")
   # (motors should still be off because our throttle is low)
   File.write(GEAR, GEAR_ON)
-  sleep 0.010
+  sleep 0.020
   check_led 1
   check_motors_are_off
 
-  # Check that motors turn on
+  puts("Check that motors turn on")
   File.write(THROTTLE, THROTTLE_MID)
-  motor1 = []
-  motor2 = []
-  motor3 = []
-  motor4 = []
-  for i in 0..100
-    motor1.push(read_fifo_num(MOTOR1))
-    motor2.push(read_fifo_num(MOTOR2))
-    motor3.push(read_fifo_num(MOTOR3))
-    motor4.push(read_fifo_num(MOTOR4))
-    sleep 0.005
-  end
-  average1 = motor1.inject(:+).to_f / motor1.size
-  average2 = motor2.inject(:+).to_f / motor2.size
-  average3 = motor3.inject(:+).to_f / motor3.size
-  average4 = motor4.inject(:+).to_f / motor4.size
-  average = (average1 + average2 + average3 + average4)/4
-  puts average1, average2, average3, average4, "(#{average})"
+  averages = get_motor_averages
+  average = (averages[0] + averages[1] + averages[2] + averages[3])/4
+  puts averages, "(#{average})"
   assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)
 
   # Check that gear switch kills the motors
   # (and that light goes off)
   File.write(GEAR, GEAR_OFF)
-  sleep 0.015
+  sleep 0.040
   check_motors_are_off
   check_led 0
 
@@ -138,7 +155,7 @@ begin
 
   # Check that we can resume flight
   File.write(GEAR, GEAR_ON)
-  sleep 0.015
+  sleep 0.040
   check_led 1
 
   sleep 1

quad/src/virt_quad/hw_impl_unix_i2c.c

 #include "hw_impl_unix.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include "iic_utils.h"
+
+void * update_i2c_input_cache(void *);
+
+union val {
+  unsigned char b[2];
+  unsigned short s;
+};
+
+static char *input_names[6];
+static int fifos[6];
+static union val cache[6];
+
+static short last_dev;
+static short last_reg;
+static short last_val;
+
+static int zero = 0;
+static int one = 1;
+static int two = 2;
+static int three = 3;
+static int four = 4;
+static int five = 5;
+
+static pthread_t worker;
 
 int unix_i2c_reset(struct I2CDriver *self) {
+  input_names[0] = "i2c-mpu-accel-x";
+  input_names[1] = "i2c-mpu-accel-y";
+  input_names[2] = "i2c-mpu-accel-z";
+  input_names[3] = "i2c-mpu-gryo-x";
+  input_names[4] = "i2c-mpu-gryo-y";
+  input_names[5] = "i2c-mpu-gyro-z";
+
+  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
+
+  // Start up worker thread whose job is to update the caches
+  pthread_create(&worker, 0, update_i2c_input_cache, &zero);
+  pthread_create(&worker, 0, update_i2c_input_cache, &one);
+  pthread_create(&worker, 0, update_i2c_input_cache, &two);
+  pthread_create(&worker, 0, update_i2c_input_cache, &three);
+  pthread_create(&worker, 0, update_i2c_input_cache, &four);
+  pthread_create(&worker, 0, update_i2c_input_cache, &five);
+
+  cache[0].s = 0;
+  cache[1].s = 0;
+  cache[2].s = 0;
+  cache[3].s = 0;
+  cache[4].s = 0;
+  cache[5].s = 0;
+
   return 0;
 }
 
@@ -8,6 +61,11 @@ int unix_i2c_write(struct I2CDriver *self,
                    unsigned short device_addr,
                    unsigned char *data,
                    unsigned int length) {
+  if (length == 2) {
+    last_dev = device_addr;
+    last_reg = data[0];
+    last_val = data[1];
+  }
   return 0;
 }
 
@@ -15,5 +73,53 @@ int unix_i2c_read(struct I2CDriver *self,
                   unsigned short device_addr,
                   unsigned char *buff,
                   unsigned int length) {
+  if (last_dev != device_addr) {
+    return -1;
+  }
+
+  switch (device_addr) {
+  case MPU9150_DEVICE_ADDR:
+    if (last_reg == ACCEL_GYRO_BASE_ADDR) {
+      buff[0] = cache[0].b[0];
+      buff[1] = cache[0].b[1];
+      buff[2] = cache[1].b[0];
+      buff[3] = cache[1].b[1];
+      buff[4] = cache[2].b[0];
+      buff[5] = cache[2].b[1];
+      buff[6] = 0;
+      buff[7] = 0;
+      buff[8] = cache[3].b[0];
+      buff[9] = cache[3].b[1];
+      buff[10] = cache[4].b[0];
+      buff[11] = cache[4].b[1];
+      buff[12] = cache[5].b[0];
+      buff[13] = cache[5].b[1];
+    }
+  }
   return 0;
 }
+
+void * update_i2c_input_cache(void *arg) {
+  int *cache_index = arg;
+  int i = *cache_index;
+  char buff[16];
+
+  // Setup FIFO
+  unlink(input_names[i]);
+  char fifoname[64];
+  sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
+  mkfifo(fifoname, 0666);
+  fifos[i] = open(fifoname, O_RDONLY);
+
+  // Block while waiting for reads
+  while (1) {
+    int bytes_read = read(fifos[i], buff, 15);
+    if (bytes_read > 0) {
+      buff[bytes_read] = '\0';
+      unsigned long val = strtoll(buff, NULL, 10);
+      cache[i].s = val;
+      printf("%s: %ld\n", input_names[i], val);
+    }
+  }
+  return NULL;
+}