From ef646e1667ac92bafea4f88c5944627db0004ea5 Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Sat, 15 Apr 2017 00:36:33 -0500
Subject: [PATCH] quad: improve virtual quad hardware layer implementation
- use shared memory instead of fifos for most I/O
- re-enable functional tests
---
quad/Makefile | 2 +-
quad/scripts/tests/test_safety_checks.rb | 47 ++++---
quad/scripts/tests/testing_library.rb | 64 +++++-----
quad/src/quad_app/initialize_components.c | 37 +++---
quad/src/quad_app/util.c | 20 ++-
quad/src/quad_app/util.h | 2 +
quad/src/virt_quad/Makefile | 2 +-
quad/src/virt_quad/hw_impl_unix.h | 17 +++
quad/src/virt_quad/hw_impl_unix_i2c.c | 107 +---------------
quad/src/virt_quad/hw_impl_unix_mio7_led.c | 42 ++-----
quad/src/virt_quad/hw_impl_unix_motor.c | 57 +--------
quad/src/virt_quad/hw_impl_unix_rc_receiver.c | 72 +----------
quad/src/virt_quad/main.c | 117 ++++++++++++++++++
13 files changed, 248 insertions(+), 338 deletions(-)
diff --git a/quad/Makefile b/quad/Makefile
index dde14bff5..b6838e4bf 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -38,7 +38,7 @@ test: all
$(MAKE) -C src/queue test
$(MAKE) -C src/computation_graph test
$(MAKE) -C src/quad_app test
- # ruby scripts/tests/test_safety_checks.rb
+ ruby scripts/tests/test_safety_checks.rb
# ruby scripts/tests/test_unix_uart.rb
# ruby scripts/tests/run_virtual_test_flight.rb
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 9e7d693e6..3e45b244a 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -16,15 +16,12 @@ Timeout::timeout(30) {
puts("Setting up...")
-
- sleep 1
-
# Start virtual quad
quad_pid = Process.spawn("./virt-quad -q",
{ :rlimit_as => 536870912, # 512 MiB total RAM
:rlimit_stack => 1048576}) # 1 MiB stack
- delay_spin_cursor(5)
+ delay_spin_cursor(1)
#################
# Begin Tests
@@ -35,8 +32,11 @@ Timeout::timeout(30) {
puts("Beginning tests...")
# Set gravity and gear
- File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
- File.write(GEAR, GEAR_OFF)
+ set_gear GEAR_OFF
+ set_flap FLAP_ON
+ `./virt-quad set rc_roll 0.498`
+ `./virt-quad set rc_pitch 0.497`
+ `./virt-quad set rc_yaw 0.498`
puts("Check that motors are off at startup")
check_motors_are_off "Motors weren't off at startup! How dangerous!"
@@ -47,7 +47,7 @@ Timeout::timeout(30) {
puts("Check that increasing the throttle does nothing to motors")
# (because gear is still off)
for i in (THROTTLE_MIN..THROTTLE_MAX).step(0.01)
- File.write(THROTTLE, i)
+ set_throttle(i)
check_motors_are_off "Was able to turn on motors with GEAR off! Yikes!"
check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
@@ -55,47 +55,44 @@ Timeout::timeout(30) {
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.5
+ sleep 0.01
check_motors_are_off "Motors turned on by gear while rc throttle was high! How dangerous!"
i = THROTTLE_MAX
while i > THROTTLE_MID
i -= 0.01
- File.write(THROTTLE, i)
+ set_throttle(i)
check_motors_are_off "Motors turned on while GEAR was off. Yikes!"
check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
end
# (swtich GEAR back to off and bring throttle off)
- File.write(GEAR, GEAR_OFF)
- File.write(THROTTLE, THROTTLE_MIN)
+ set_gear GEAR_OFF
+ set_throttle THROTTLE_MIN
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.5
+ set_gear GEAR_ON
+ sleep 0.01
check_motors_are_off "Motors turned on while throttle was low! Yikes!"
check_led(1, "LED didn't turn on when GEAR was switched on!")
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)
+ set_throttle THROTTLE_MID
+ spinner = Thread.new { delay_spin_cursor(5) }
+ motors = get_motor_averages(100, 5)
+ spinner.exit
+ p motors
+ motors.each { |value| assert_operator(value, :>, THROTTLE_EIGHTH) }
puts("Check that gear switch kills the motors")
- # (and that light goes off)
- File.write(GEAR, GEAR_OFF)
- sleep 0.5
+ set_gear GEAR_OFF
+ sleep 0.01
check_motors_are_off "Motors didn't turn off when GEAR was switched off! How dangerous!"
- #check_led 0
# (Bring the RC throttle back down)
- File.write(THROTTLE, THROTTLE_MIN)
+ set_throttle THROTTLE_MIN
- sleep 1
puts "All safety checks passed."
ensure
diff --git a/quad/scripts/tests/testing_library.rb b/quad/scripts/tests/testing_library.rb
index 5324eacdd..9da10aef8 100644
--- a/quad/scripts/tests/testing_library.rb
+++ b/quad/scripts/tests/testing_library.rb
@@ -9,6 +9,8 @@ MOTOR_MIN = 0.0
MOTOR_MAX = 1.0
GEAR_ON = 0.70800
GEAR_OFF = 0.18300
+FLAP_ON = 0.9
+FLAP_OFF = 0.1
GRAVITY = 4096
MOTOR1 = "virt-quad-fifos/motor1"
@@ -33,47 +35,41 @@ require 'timeout'
require 'thread'
include Test::Unit::Assertions
-$fifos = Hash.new
+# Utility functions
+def check_motors_are_off(msg)
+ motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ motors.each { |val|
+ assert_operator(val, :<=, THROTTLE_MIN, msg)
+ }
+end
-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
+def check_led(is_on, msg)
+ led = `./virt-quad get led`.to_i
+ assert_equal(is_on, led, msg)
end
-# Utility functions
-def check_motors_are_off(msg)
- 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, msg)
- assert_operator(motor2, :<=, THROTTLE_MIN, msg)
- assert_operator(motor3, :<=, THROTTLE_MIN, msg)
- assert_operator(motor4, :<=, THROTTLE_MIN, msg)
+def set_gear(gear)
+ `./virt-quad set rc_gear #{gear}`
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
+def set_flap(flap)
+ `./virt-quad set rc_flap #{flap}`
end
-def check_led(is_on, msg)
- led = read_fifo_num(LED)
- assert_equal(is_on, led, msg)
+def set_throttle(throttle)
+ `./virt-quad set rc_throttle #{throttle}`
+end
+
+def get_motor_averages(times, total_time)
+ motor_sums = [0.0, 0.0, 0.0, 0.0]
+ for i in 0..times
+ motors = `./virt-quad get motors`.chomp.split("\n").map { |s| s.to_f }
+ for i in 0..3
+ motor_sums[i] += motors[i]
+ end
+ sleep (total_time.to_f / times.to_f)
+ end
+ motor_sums.map {|val| val / times}
end
def delay_spin_cursor(delay)
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 5cd49342d..6e85fa4a1 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -15,26 +15,27 @@
extern int Xil_AssertWait;
+int start_condition_is_met(float *rc_commands) {
+ return rc_commands[THROTTLE] < 0.125 &&
+ gear_is_engaged(rc_commands[GEAR]) &&
+ flap_is_engaged(rc_commands[FLAP]);
+}
+
int protection_loops(modular_structs_t *structs)
{
- float rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
-
- struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
- read_rec_all(rc_receiver, rc_commands);
-
- while(rc_commands[THROTTLE] < 0.075 || // wait for RC receiver to connect to transmitter
- rc_commands[THROTTLE] > 0.125 || // Wait until throttle is low
- read_kill(rc_commands[GEAR]) || // Wait until gear switch is engaged (so you don't immediately break out of the main loop below)
- !read_flap(rc_commands[FLAP])) // Wait for the flight mode to be set to manual
- {
- process_received(structs);
- read_rec_all(rc_receiver, rc_commands);
- }
-
- // let the pilot/observers know that the quad is now active
- MIO7_led_on();
-
- return 0;
+ float rc_commands[6];
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ read_rec_all(rc_receiver, rc_commands);
+
+ while(!start_condition_is_met(rc_commands)) {
+ process_received(structs);
+ read_rec_all(rc_receiver, rc_commands);
+ }
+
+ // let the pilot/observers know that the quad is now active
+ MIO7_led_on();
+
+ return 0;
}
int init_structs(modular_structs_t *structs) {
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index e982432ea..698d453c7 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -49,15 +49,23 @@ int hexStrToInt(char *buf, int startIdx, int endIdx) {
* Otherwise, do nothing
*/
int read_kill(float gear) {
- if(gear < GEAR_MID)
- return 1;
- return 0;
+ if (gear_is_engaged(gear)) return 0;
+ else return 1;
+}
+
+int gear_is_engaged(float gear) {
+ if (gear > GEAR_MID) return 1;
+ else return 0;
}
int read_flap(float flap) {
- if(flap > FLAP_MID)
- return 1;
- return 0;
+ if (flap_is_engaged(flap)) return 1;
+ else return 0;
+}
+
+int flap_is_engaged(float flap) {
+ if (flap > FLAP_MID) return 1;
+ else return 0;
}
/**
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 390bafd9d..d67542829 100644
--- a/quad/src/quad_app/util.h
+++ b/quad/src/quad_app/util.h
@@ -14,6 +14,8 @@
void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer);
int read_kill(float gear);
int read_flap(float flap);
+int gear_is_engaged(float gear);
+int flap_is_engaged(float flap);
void kill_motors(struct MotorDriver *motors);
int build_int(u8 *buff);
diff --git a/quad/src/virt_quad/Makefile b/quad/src/virt_quad/Makefile
index acf6d27cb..241f5e48b 100644
--- a/quad/src/virt_quad/Makefile
+++ b/quad/src/virt_quad/Makefile
@@ -1,7 +1,7 @@
TOP=../..
NAME = virt-quad
-REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread
+REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread -lrt
include $(TOP)/executable.mk
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 85e223f28..296d30fbf 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -9,9 +9,26 @@
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include "controllers.h"
+#include <pthread.h>
+#include <errno.h>
#define VIRT_QUAD_FIFOS_DIR "virt-quad-fifos"
+struct VirtQuadIO {
+ pthread_mutex_t led_lock;
+ int led;
+ pthread_mutex_t motors_lock;
+ float motors[4];
+ pthread_mutex_t rc_lock;
+ float rc_receiver[6];
+};
+
+#define VIRT_QUAD_SHARED_MEMORY "/virt-quad-io"
+
int unix_uart_reset(struct UARTDriver *self);
int unix_uart_write(struct UARTDriver *self, unsigned char c);
int unix_uart_read(struct UARTDriver *self, unsigned char *c);
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index 97228f0ba..aa2c1d5ce 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -7,49 +7,7 @@
#define NUM_INPUTS 7
-void * update_i2c_input_cache(void *);
-
-union val {
- unsigned char b[2];
- unsigned short s;
-};
-
-static char *input_names[NUM_INPUTS];
-static int fifos[NUM_INPUTS];
-static union val cache[NUM_INPUTS];
-
-static short last_dev;
-static short last_reg;
-static short last_val;
-
-static int nums[] = {0, 1, 2, 3, 4, 5};
-static pthread_t workers[NUM_INPUTS];
-
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";
- input_names[6] = "i2c-lidar";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-
- // Start up worker thread whose job is to update the caches
- int i;
- for (i = 0; i < NUM_INPUTS; i += 1) {
- pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
- }
-
- cache[0].s = 0;
- cache[1].s = 0;
- cache[2].s = 0;
- cache[3].s = 0;
- cache[4].s = 0;
- cache[5].s = 0;
- cache[6].s = 0;
-
return 0;
}
@@ -57,73 +15,12 @@ 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;
+ return -1;
}
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];
- }
- else if (last_reg == LIDARLITE_DEVICE_ADDR) {
- buff[0] = cache[6].b[0];
- buff[1] = cache[6].b[0];
- }
- else {
- return -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);
- }
- pthread_yield();
- }
- return NULL;
+ return -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 b9b66dd09..af831c580 100644
--- a/quad/src/virt_quad/hw_impl_unix_mio7_led.c
+++ b/quad/src/virt_quad/hw_impl_unix_mio7_led.c
@@ -7,25 +7,10 @@
#include <unistd.h>
#include <pthread.h>
-void * output_cached_led();
-
-int on;
-static char *led_fifo_name;
-pthread_t worker;
+extern struct VirtQuadIO *virt_quad_io;
+static int on;
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;
}
@@ -33,6 +18,11 @@ int unix_mio7_led_turn_on(struct LEDDriver *self) {
if (!on) {
puts("LED ON");
on = 1;
+
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->led_lock);
+ io->led = on;
+ pthread_mutex_unlock(&io->led_lock);
}
return 0;
}
@@ -41,17 +31,11 @@ int unix_mio7_led_turn_off(struct LEDDriver *self) {
if (on) {
puts("LED OFF");
on = 0;
- }
- return 0;
-}
-void * output_cached_led() {
- char buff[16];
- while (1) {
- int fifo = open(led_fifo_name, O_WRONLY);
- sprintf(buff, "%d\n", on);
- write(fifo, buff, strlen(buff));
- close(fifo);
- pthread_yield();
- }
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->led_lock);
+ io->led = on;
+ pthread_mutex_unlock(&io->led_lock);
+}
+ return 0;
}
diff --git a/quad/src/virt_quad/hw_impl_unix_motor.c b/quad/src/virt_quad/hw_impl_unix_motor.c
index 3ad271b52..98da452a0 100644
--- a/quad/src/virt_quad/hw_impl_unix_motor.c
+++ b/quad/src/virt_quad/hw_impl_unix_motor.c
@@ -6,65 +6,18 @@
#include <pthread.h>
#include <unistd.h>
-void * output_cache();
-
-static char *output_pwms[4];
-static float cache[4];
-pthread_t worker;
-static void float_equals(float a, float b);
-
-static int zero = 0;
-static int one = 1;
-static int two = 2;
-static int three = 3;
+extern struct VirtQuadIO *virt_quad_io;
int unix_motor_reset(struct MotorDriver *self) {
- output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/motor1";
- output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/motor2";
- output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/motor3";
- output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/motor4";
-
- mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
-
- // Start up worker thread whose job is to update the caches
- 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;
}
int unix_motor_write(struct MotorDriver *self,
unsigned int channel,
float magnitude) {
- if (cache[channel] != magnitude) {
- printf("%s: %.2f\n", output_pwms[channel], magnitude);
- }
- cache[channel] = magnitude;
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->motors_lock);
+ io->motors[channel] = magnitude;
+ pthread_mutex_unlock(&io->motors_lock);
return 0;
}
-
-void * output_cache(void *arg) {
- int *output_index = arg;
- char buff[16];
- 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, "%.2f\n", cache[i]);
- write(fifo, buff, strlen(buff));
- close(fifo);
- pthread_yield();
- }
- return NULL;
-}
-
-static void float_equals(float a, float b) {
- return fabs(a - b) < 0.00001;
-}
diff --git a/quad/src/virt_quad/hw_impl_unix_rc_receiver.c b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
index 229a019cd..85cf852c7 100644
--- a/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
+++ b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
@@ -5,80 +5,18 @@
#include <fcntl.h>
#include <pthread.h>
-void * update_input_cache();
-
-static char *input_names[6];
-static int fifos[6];
-static float cache[6];
-static pthread_t workers[6];
-static int nums[] = {0, 1, 2, 3, 4, 5};
+extern struct VirtQuadIO *virt_quad_io;
int unix_rc_receiver_reset(struct RCReceiverDriver *self) {
- input_names[0] = "rc-throttle";
- input_names[1] = "rc-roll";
- input_names[2] = "rc-pitch";
- input_names[3] = "rc-yaw";
- input_names[4] = "rc-gear";
- input_names[5] = "rc-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) {
- pthread_create(&workers[i], 0, update_input_cache, &nums[i]);
- usleep(1000);
- }
-
- cache[0] = THROTTLE_MIN;
- cache[1] = ROLL_CENTER;
- cache[2] = PITCH_CENTER;
- cache[3] = YAW_CENTER;
- cache[4] = GEAR_0;
- cache[5] = FLAP_1;
-
- for (i = 0; i < 6; i += 1) {
- printf("%s: %.2f\n", input_names[i], cache[i]);
- }
-
return 0;
}
int unix_rc_receiver_read(struct RCReceiverDriver *self,
unsigned int channel,
float *magnitude) {
-
- *magnitude = cache[channel];
+ struct VirtQuadIO *io = virt_quad_io;
+ pthread_mutex_lock(&io->rc_lock);
+ *magnitude = io->rc_receiver[channel];
+ pthread_mutex_unlock(&io->rc_lock);
return 0;
}
-
-void * update_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';
- float val = strtof(buff, NULL);
- if (val <= 1 && val >= 0) {
- cache[i] = val;
- printf("%s: %.2f\n", input_names[i], val);
- }
- else {
- printf("%s: Bad value (%f) - input not received\n", input_names[i], val);
- }
- }
- pthread_yield();
- }
- return NULL;
-}
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index 5f6aaf8db..7f9e4447e 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -4,6 +4,13 @@
#include "quad_app.h"
#include <fcntl.h>
+int handle_io_output(const char *name);
+int handle_io_input(const char *name, const char *value_str);
+void set_shm(float value, float *dest, pthread_mutex_t *lock);
+
+struct VirtQuadIO *virt_quad_io;
+int virt_quad_io_file_descriptor;
+
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_unix_i2c();
hardware->rc_receiver = create_unix_rc_receiver();
@@ -30,7 +37,117 @@ int main(int argc, char *argv[]) {
exit(EXIT_FAILURE);
}
}
+ argc -= optind;
+ argv += optind;
+
+
+ // Decide if we are launching the quad or responding to a request
+ // about a running quad
+ if (argv[0] == NULL) {
+ // Prepare the shared memory for io. Clear memory and initialize.
+ puts("Setting up...");
+ int *fd = &virt_quad_io_file_descriptor;
+ *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR | O_TRUNC, 0666);
+ ftruncate(*fd, sizeof(struct VirtQuadIO));
+ virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+ pthread_mutexattr_t attr;
+ pthread_mutexattr_init(&attr);
+ pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+ pthread_mutex_init(&virt_quad_io->led_lock, &attr);
+ pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
+ pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
+ puts("Finished setting up.");
+ }
+ else {
+ // Open exising shared memory for io. DO NOT CLEAR.
+ int *fd = &virt_quad_io_file_descriptor;
+ *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR, 0666);
+ virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+
+ // Meet requests
+ if (strcmp(argv[0], "get") == 0 && argv[1] != NULL) {
+ handle_io_output(argv[1]);
+ }
+ else if (strcmp(argv[0], "set") == 0 && argv[1] != NULL && argv[2] != NULL) {
+ handle_io_input(argv[1], argv[2]);
+ }
+ return 0;
+ }
+ puts("Starting the quad application");
quad_main(setup_hardware);
return 0;
}
+
+/**
+ * The user wants to read an output by name. Get the output
+ * and print to stdout.
+ */
+int handle_io_output(const char *name) {
+ int ret = 0;
+ struct VirtQuadIO *io = virt_quad_io;
+ if (strcmp(name, "led") == 0) {
+ pthread_mutex_lock(&io->led_lock);
+ printf("%d\n", io->led);
+ pthread_mutex_unlock(&io->led_lock);
+ }
+ else if (strcmp(name, "motors") == 0) {
+ pthread_mutex_lock(&io->motors_lock);
+ int i;
+ for (i = 0; i < 4; i += 1) {
+ printf("%f\n", io->motors[i]);
+ }
+ pthread_mutex_unlock(&io->motors_lock);
+ }
+ else {
+ puts("Given output not recognized.");
+ ret = -1;
+ }
+ return ret;
+}
+
+/**
+ * The user wants to set an input by name, and has supplied
+ * a value. Set the appropriate input to the given value.
+ */
+int handle_io_input(const char *name, const char *value_str) {
+ int ret = 0;
+ errno = 0;
+ float value = strtof(value_str, NULL);
+ if (errno) {
+ ret = -1;
+ puts("Given number format was bad.");
+ }
+ else {
+ struct VirtQuadIO *io = virt_quad_io;
+ if (strcmp(name, "rc_throttle") == 0) {
+ set_shm(value, &io->rc_receiver[THROTTLE], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_roll") == 0) {
+ set_shm(value, &io->rc_receiver[ROLL], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_pitch") == 0) {
+ set_shm(value, &io->rc_receiver[PITCH], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_yaw") == 0) {
+ set_shm(value, &io->rc_receiver[YAW], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_gear") == 0) {
+ set_shm(value, &io->rc_receiver[GEAR], &io->rc_lock);
+ }
+ else if (strcmp(name, "rc_flap") == 0) {
+ set_shm(value, &io->rc_receiver[FLAP], &io->rc_lock);
+ }
+ else {
+ puts("Given input not recognized.");
+ ret = -1;
+ }
+ }
+ return ret;
+}
+
+void set_shm(float value, float *dest, pthread_mutex_t *lock) {
+ pthread_mutex_lock(lock);
+ *dest = value;
+ pthread_mutex_unlock(lock);
+}
--
GitLab