quad: add functional safety quad tests to CI

- uses virtual quad

ci-test.sh

@@ -2,5 +2,10 @@
 
 set -e
 
-# Quad
-cd quad && make && make test
+# Quad Library Unit tests
+(cd quad && make && make test)
+
+# Quad functional safety tests
+export PATH=/usr/local/bin:$PATH
+echo $(ruby --version)
+ruby quad/scripts/tests/test_safety_checks.rb

quad/scripts/tests/test_safety_checks.rb

+#!/usr/bin/env ruby
+
+# Safety Checks
+#
+# Startup the virtual quad and make sure it doesn't allow combinations of things
+# that could hurt people.
+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
+
+require 'test/unit/assertions'
+require 'thread'
+include Test::Unit::Assertions
+
+# 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
+end
+
+def check_led(on)
+  led = File.read("virt-quad-fifos/mio7-led")
+  assert_equal(led.to_i, on)
+end
+
+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 Tests
+#################
+
+begin
+
+  # Check that motors are off at startup
+  check_motors_are_off
+
+  # Check that LED is off at startup
+  check_led(0)
+
+  # 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)
+    check_motors_are_off
+    sleep 0.005
+  end
+
+  # 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)
+  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)
+    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)
+
+  # 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
+  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})"
+  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
+  check_motors_are_off
+  check_led 0
+
+  # (Bring the RC throttle back down)
+  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MIN)
+
+  # Check that we can resume flight
+  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
+  sleep 0.015
+  check_led 1
+
+  sleep 1
+  puts "All safety checks passed."
+
+ensure
+
+  Process.kill(9, quad)
+
+end
+

quad/src/quad_app/controllers.h

@@ -102,6 +102,9 @@
 #define min 100000
 #define max 200000
 
+#define MOTOR_MIN 100000
+#define MOTOR_MAX 200000
+
 void filter_PWMs(int* mixer);
 void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
 void Aero_to_PWMS(int* PWMs, int* aero);

quad/src/quad_app/initialize_components.c

@@ -7,6 +7,7 @@
  
 #include "initialize_components.h"
 #include "communication.h"
+#include "controllers.h"
 #include "sensor.h"
 #include "iic_utils.h"
 
@@ -83,6 +84,12 @@ int init_structs(modular_structs_t *structs) {
   struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
   if (pwm_outputs->reset(pwm_outputs)) return -1;
 
+  // Set motor outputs to off
+  int i;
+  for (i = 0; i < 4; i += 1) {
+    pwm_outputs->write(pwm_outputs, i, MOTOR_MIN);
+  }
+
   // Initialize sensors
 
   //manual flight mode

quad/src/quad_app/util.c

@@ -68,10 +68,10 @@ int read_flap(int flap)
  * Turns off the motors
  */
 void kill_motors(struct PWMOutputDriver *pwm_outputs) {
-  pwm_outputs->write(pwm_outputs, 0, 0);
-  pwm_outputs->write(pwm_outputs, 1, 0);
-  pwm_outputs->write(pwm_outputs, 2, 0);
-  pwm_outputs->write(pwm_outputs, 3, 0);
+  pwm_outputs->write(pwm_outputs, 0, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 1, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 2, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 3, MOTOR_MIN);
 }
 
 int build_int(u8 *buff) {

quad/src/virt_quad/Makefile

 TOP=../..
 
 NAME = virt-quad
-REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm
+REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread
 
 include $(TOP)/executable.mk
 

quad/src/virt_quad/hw_impl_unix_mio7_led.c

 #include "hw_impl_unix.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <string.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <pthread.h>
+
+void * output_cached_led();
 
 int on;
+static char *led_fifo_name;
+pthread_t worker;
 
 int unix_mio7_led_reset(struct LEDDriver *self) {
+  led_fifo_name = VIRT_QUAD_FIFOS_DIR "/mio7-led";
+
+  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
+  int i;
+  for (i = 0; i < 4; i += 1) {
+    unlink(led_fifo_name);
+    mkfifo(led_fifo_name, 0666);
+  }
+
+  // Start up worker thread whose job is to update the caches
+  pthread_create(&worker, 0, output_cached_led, NULL);
   return 0;
 }
 
@@ -21,3 +43,16 @@ int unix_mio7_led_turn_off(struct LEDDriver *self) {
   }
   return 0;
 }
+
+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));
+    }
+    close(fifo);
+  }
+}

quad/src/virt_quad/hw_impl_unix_pwm_input.c

@@ -3,29 +3,36 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
+#include <pthread.h>
 
-static char *pwms[6];
+void * update_input_caches();
+
+static char *input_names[6];
 static int fifos[6];
 static unsigned long cache[6];
+pthread_t worker;
 
 int unix_pwm_input_reset(struct PWMInputDriver *self) {
-  pwms[0] = "pwm-input-throttle";
-  pwms[1] = "pwm-input-roll";
-  pwms[2] = "pwm-input-pitch";
-  pwms[3] = "pwm-input-yaw";
-  pwms[4] = "pwm-input-gear";
-  pwms[5] = "pwm-input-flap";
+  input_names[0] = "pwm-input-throttle";
+  input_names[1] = "pwm-input-roll";
+  input_names[2] = "pwm-input-pitch";
+  input_names[3] = "pwm-input-yaw";
+  input_names[4] = "pwm-input-gear";
+  input_names[5] = "pwm-input-flap";
 
   mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
   int i;
   for (i = 0; i < 6; i += 1) {
-    unlink(pwms[i]);
+    unlink(input_names[i]);
     char fifoname[64];
-    sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, pwms[i]);
+    sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
     mkfifo(fifoname, 0666);
     fifos[i] = open(fifoname, O_RDONLY | O_NONBLOCK);
   }
 
+  // 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;
@@ -34,7 +41,7 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
   cache[5] = FLAP_1;
 
   for (i = 0; i < 6; i += 1) {
-    printf("%s: %d\n", pwms[i], cache[i]);
+    printf("%s: %d\n", input_names[i], cache[i]);
   }
 
   return 0;
@@ -44,17 +51,25 @@ int unix_pwm_input_read(struct PWMInputDriver *self,
                         unsigned int channel,
                         unsigned long *pulse_width_us) {
 
+  *pulse_width_us = cache[channel];
+  return 0;
+}
+
+void * update_input_caches() {
   char buff[16];
-  int bytes_read = read(fifos[channel], buff, 15);
-  if (bytes_read >= 6) {
-    buff[bytes_read] = '\0';
-    unsigned long val = strtoll(buff, NULL, 10);
-    if (val < max && val > min) {
-      cache[channel] = val;
-      printf("%s: %d\n", pwms[channel], val);
+  int i;
+  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);
+        }
+      }
     }
   }
-
-  *pulse_width_us = cache[channel];
-  return 0;
+  return NULL;
 }

quad/src/virt_quad/hw_impl_unix_pwm_output.c

@@ -3,8 +3,14 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <string.h>
+#include <pthread.h>
+#include <unistd.h>
+
+void * output_caches();
 
 static char *output_pwms[4];
+static unsigned long cache[4];
+pthread_t worker;
 
 int unix_pwm_output_reset(struct PWMOutputDriver *self) {
   output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
@@ -19,18 +25,35 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
     mkfifo(output_pwms[i], 0666);
   }
 
+  // Start up worker thread whose job is to update the caches
+  pthread_create(&worker, 0, output_caches, NULL);
+
   return 0;
 }
 
 int unix_pwm_output_write(struct PWMOutputDriver *self,
                           unsigned int channel,
                           unsigned long pulse_width_us) {
-  char buff[16];
-  int fifo = open(output_pwms[channel], O_WRONLY | O_NONBLOCK);
-  if (fifo >= 0) {
-    sprintf(buff, "%d\0", pulse_width_us);
-    write(fifo, buff, strlen(buff));
+  if (cache[channel] != pulse_width_us) {
+    printf("%s: %ld\n", output_pwms[channel], pulse_width_us);
   }
-  close(fifo);
+  cache[channel] = pulse_width_us;
   return 0;
 }
+
+void * output_caches() {
+  char buff[16];
+  int i;
+  while (1) {
+    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;
+}