quad: update fifos to use blocking I/O for inputs and outputs

quad/scripts/tests/test_safety_checks.rb

@@ -16,25 +16,39 @@ MOTOR_MAX = 200000
 GEAR_ON = 170800
 GEAR_OFF = 118300
 
+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"
+
 require 'test/unit/assertions'
 require 'thread'
 include Test::Unit::Assertions
 
+def read_fifo_num(fifo)
+  File.read(fifo).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 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__))
@@ -61,47 +75,47 @@ begin
   # 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
   # (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
   # (motors should still be off because our throttle is low)
-  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
+  File.write(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)
+  File.write(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)
+    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
@@ -114,16 +128,16 @@ begin
 
   # Check that gear switch kills the motors
   # (and that light goes off)
-  File.write("virt-quad-fifos/pwm-input-gear", GEAR_OFF)
+  File.write(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)
+  File.write(THROTTLE, THROTTLE_MIN)
 
   # Check that we can resume flight
-  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
+  File.write(GEAR, GEAR_ON)
   sleep 0.015
   check_led 1
 

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;
 }