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

groundStation/scripts/bypass_kill_switch.sh

+#! /bin/bash
+cd ..
+while true ; do
+	./setparam 37 0 $1
+	sleep 0.4
+	./setparam 37 0 $(( $1 - 1 ))
+	sleep 0.4
+done

groundStation/scripts/parameterize.sh

+cd ..
+./addnode 0 "Zero"
+./setparam "zero" 0 0
+./setsource "signal mixer" "pitch" "zero" 0
+./setsource "signal mixer" "roll" "zero" 0
+./setsource "signal mixer" "yaw" "zero" 0
+./addnode 0 "PWM_VAL"
+./setparam "pwm_val" 0 100000
+./setsource "signal mixer" "throttle" "pwm_val" 0
+

quad/executable.mk

@@ -23,7 +23,7 @@ CLEANUP = $(TARGET) $(OBJDIR)
 default: $(TARGET)
 
 run: $(TARGET)
-	cd $(EXEDIR) && ./$(NAME)
+	$(EXEDIR)/$(NAME)
 
 clean:
 	rm -rf $(CLEANUP)

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,26 +15,67 @@ 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"
+
+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
 
+$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
 def check_motors_are_off
-  motor1 = File.read("virt-quad-fifos/pwm-output-motor1")
-  motor2 = File.read("virt-quad-fifos/pwm-output-motor2")
-  motor3 = File.read("virt-quad-fifos/pwm-output-motor3")
-  motor4 = File.read("virt-quad-fifos/pwm-output-motor4")
-  assert_operator motor1.to_i, :<=, THROTTLE_MIN
-  assert_operator motor2.to_i, :<=, THROTTLE_MIN
-  assert_operator motor3.to_i, :<=, THROTTLE_MIN
-  assert_operator motor4.to_i, :<=, THROTTLE_MIN
+  motor1 = read_fifo_num(MOTOR1)
+  motor2 = read_fifo_num(MOTOR2)
+  motor3 = read_fifo_num(MOTOR3)
+  motor4 = read_fifo_num(MOTOR4)
+  assert_operator motor1, :<=, THROTTLE_MIN
+  assert_operator motor2, :<=, THROTTLE_MIN
+  assert_operator motor3, :<=, THROTTLE_MIN
+  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 = File.read("virt-quad-fifos/mio7-led")
-  assert_equal(led.to_i, on)
+  led = read_fifo_num(LED)
+  assert_equal(led, on)
 end
 
 script_dir = File.expand_path(File.dirname(__FILE__))
@@ -51,80 +92,70 @@ 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("virt-quad-fifos/pwm-input-throttle", i)
+    File.write(THROTTLE, i)
     check_motors_are_off
     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("virt-quad-fifos/pwm-input-gear", GEAR_ON)
+  File.write(GEAR, GEAR_ON)
   sleep 0.015
   check_motors_are_off
   i = THROTTLE_MAX
   while i > THROTTLE_MID
     i -= 1000
-    File.write("virt-quad-fifos/pwm-input-throttle", i)
+    File.write(THROTTLE, i)
     check_motors_are_off
     check_led 0
     sleep 0.005
   end
 
   # (swtich GEAR back to off and bring throttle off)
-  File.write("virt-quad-fifos/pwm-input-gear", GEAR_OFF)
-  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MIN)
+  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("virt-quad-fifos/pwm-input-gear", GEAR_ON)
-  sleep 0.010
+  File.write(GEAR, GEAR_ON)
+  sleep 0.020
   check_led 1
   check_motors_are_off
 
-  # Check that motors turn on
-  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MID)
-  motor1 = []
-  motor2 = []
-  motor3 = []
-  motor4 = []
-  for i in 0..100
-    motor1.push(File.read("virt-quad-fifos/pwm-output-motor1").to_i)
-    motor2.push(File.read("virt-quad-fifos/pwm-output-motor2").to_i)
-    motor3.push(File.read("virt-quad-fifos/pwm-output-motor3").to_i)
-    motor4.push(File.read("virt-quad-fifos/pwm-output-motor4").to_i)
-    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})"
+  puts("Check that motors turn on")
+  File.write(THROTTLE, THROTTLE_MID)
+  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("virt-quad-fifos/pwm-input-gear", GEAR_OFF)
-  sleep 0.015
+  File.write(GEAR, GEAR_OFF)
+  sleep 0.040
   check_motors_are_off
   check_led 0
 
   # (Bring the RC throttle back down)
-  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MIN)
+  File.write(THROTTLE, THROTTLE_MIN)
 
   # Check that we can resume flight
-  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
-  sleep 0.015
+  File.write(GEAR, GEAR_ON)
+  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;
+}

quad/src/virt_quad/hw_impl_unix_mio7_led.c

@@ -25,6 +25,7 @@ int unix_mio7_led_reset(struct LEDDriver *self) {
 
   // Start up worker thread whose job is to update the caches
   pthread_create(&worker, 0, output_cached_led, NULL);
+
   return 0;
 }
 
@@ -47,12 +48,10 @@ int unix_mio7_led_turn_off(struct LEDDriver *self) {
 void * output_cached_led() {
   char buff[16];
   while (1) {
-    usleep(500); // don't spam the reader
-    int fifo = open(led_fifo_name, O_WRONLY | O_NONBLOCK);
-    if (fifo >= 0) {
-      sprintf(buff, "%d ", on);
-      write(fifo, buff, strlen(buff));
-    }
+    int fifo = open(led_fifo_name, O_WRONLY);
+    sprintf(buff, "%d\n", on);
+    write(fifo, buff, strlen(buff));
     close(fifo);
+    usleep(500); // don't spam the reader
   }
 }

quad/src/virt_quad/hw_impl_unix_pwm_input.c

@@ -5,7 +5,7 @@
 #include <fcntl.h>
 #include <pthread.h>
 
-void * update_input_caches();
+void * update_input_cache();
 
 static char *input_names[6];
 static int fifos[6];
@@ -21,18 +21,13 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
   input_names[5] = "pwm-input-flap";
 
   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) {
-    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 | O_NONBLOCK);
+    pthread_create(&worker, 0, update_input_cache, &i);
   }
 
-  // Start up worker thread whose job is to update the caches
-  pthread_create(&worker, 0, update_input_caches, NULL);
-
   cache[0] = THROTTLE_MIN;
   cache[1] = ROLL_CENTER;
   cache[2] = PITCH_CENTER;
@@ -55,19 +50,30 @@ int unix_pwm_input_read(struct PWMInputDriver *self,
   return 0;
 }
 
-void * update_input_caches() {
+void * update_input_cache(void *arg) {
+  int *cache_index = arg;
+  int i = *cache_index;
   char buff[16];
-  int i;
+
+  // 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) {
-    for (i = 0; i < 6; i += 1) {
-      int bytes_read = read(fifos[i], buff, 15);
-      if (bytes_read >= 6) {
-        buff[bytes_read] = '\0';
-        unsigned long val = strtoll(buff, NULL, 10);
-        if (val < max && val > min) {
-          cache[i] = val;
-          printf("%s: %d\n", input_names[i], val);
-        }
+    int bytes_read = read(fifos[i], buff, 15);
+    if (bytes_read > 0) {
+      buff[bytes_read] = '\0';
+      unsigned long val = strtoll(buff, NULL, 10);
+      if (val < max && val > min) {
+	cache[i] = val;
+	printf("%s: %d\n", input_names[i], val);
+      }
+      else {
+	printf("%s: Bad value - input not received\n", input_names[i]);
       }
     }
   }

quad/src/virt_quad/hw_impl_unix_pwm_output.c

@@ -6,12 +6,17 @@
 #include <pthread.h>
 #include <unistd.h>
 
-void * output_caches();
+void * output_cache();
 
 static char *output_pwms[4];
 static unsigned long cache[4];
 pthread_t worker;
 
+static int zero = 0;
+static int one = 1;
+static int two = 2;
+static int three = 3;
+
 int unix_pwm_output_reset(struct PWMOutputDriver *self) {
   output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
   output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor2";
@@ -19,14 +24,12 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
   output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor4";
 
   mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-  int i;
-  for (i = 0; i < 4; i += 1) {
-    unlink(output_pwms[i]);
-    mkfifo(output_pwms[i], 0666);
-  }
 
   // Start up worker thread whose job is to update the caches
-  pthread_create(&worker, 0, output_caches, NULL);
+  pthread_create(&worker, 0, output_cache, &zero);
+  pthread_create(&worker, 0, output_cache, &one);
+  pthread_create(&worker, 0, output_cache, &two);
+  pthread_create(&worker, 0, output_cache, &three);
 
   return 0;
 }
@@ -41,19 +44,22 @@ int unix_pwm_output_write(struct PWMOutputDriver *self,
   return 0;
 }
 
-void * output_caches() {
+void * output_cache(void *arg) {
+  int *output_index = arg;
   char buff[16];
-  int i;
+  int i = *output_index;
+
+  // Setup FIFO
+  unlink(output_pwms[i]);
+  mkfifo(output_pwms[i], 0666);
+
+  // Block while waiting for someone to listen
   while (1) {
+    int fifo = open(output_pwms[i], O_WRONLY);
+    sprintf(buff, "%ld\n", cache[i]);
+    write(fifo, buff, strlen(buff));
+    close(fifo);
     usleep(500); // don't spam the reader
-    for (i = 0; i < 4; i += 1) {
-      int fifo = open(output_pwms[i], O_WRONLY | O_NONBLOCK);
-      if (fifo >= 0) {
-        sprintf(buff, " %ld ", cache[i]);
-        write(fifo, buff, strlen(buff));
-      }
-      close(fifo);
-    }
   }
   return NULL;
 }