From efad6f9125708199fc21fc8a208444e4941085de Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Fri, 31 Mar 2017 18:42:24 -0500
Subject: [PATCH] quad: update fifos to use blocking I/O for inputs and outputs
---
quad/scripts/tests/test_safety_checks.rb | 62 ++++++++++++--------
quad/src/virt_quad/hw_impl_unix_mio7_led.c | 11 ++--
quad/src/virt_quad/hw_impl_unix_pwm_input.c | 46 ++++++++-------
quad/src/virt_quad/hw_impl_unix_pwm_output.c | 40 +++++++------
4 files changed, 92 insertions(+), 67 deletions(-)
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 2ed03ed7f..b5075df08 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/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
diff --git a/quad/src/virt_quad/hw_impl_unix_mio7_led.c b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
index 521542378..6aef66752 100644
--- a/quad/src/virt_quad/hw_impl_unix_mio7_led.c
+++ b/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
}
}
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_input.c b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
index 75805653c..b48746109 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_input.c
+++ b/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]);
}
}
}
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_output.c b/quad/src/virt_quad/hw_impl_unix_pwm_output.c
index 03f9815cf..71b89c4f4 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_output.c
+++ b/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;
}
--
GitLab