From 5bfc14544d83b889587d30356ba67569d7f4e0e7 Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Fri, 31 Mar 2017 21:03:38 -0500
Subject: [PATCH] quad: create tests script for virtual quad
And add I2C implementation for virtual quad
---
quad/scripts/tests/run_virtual_test_flight.rb | 106 ++++++++++++++++++
quad/scripts/tests/test_safety_checks.rb | 73 +++++++-----
quad/src/virt_quad/hw_impl_unix_i2c.c | 106 ++++++++++++++++++
3 files changed, 257 insertions(+), 28 deletions(-)
create mode 100644 quad/scripts/tests/run_virtual_test_flight.rb
diff --git a/quad/scripts/tests/run_virtual_test_flight.rb b/quad/scripts/tests/run_virtual_test_flight.rb
new file mode 100644
index 000000000..9bc3afa36
--- /dev/null
+++ b/quad/scripts/tests/run_virtual_test_flight.rb
@@ -0,0 +1,106 @@
+#!/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
+
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index b5075df08..f4e690495 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -15,6 +15,7 @@ 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"
@@ -26,12 +27,21 @@ 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
-def read_fifo_num(fifo)
- File.read(fifo).chomp.split("\n").last.to_i
+$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
@@ -46,6 +56,23 @@ def check_motors_are_off
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 = read_fifo_num(LED)
assert_equal(led, on)
@@ -65,14 +92,18 @@ 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(THROTTLE, i)
@@ -80,7 +111,7 @@ begin
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(GEAR, GEAR_ON)
sleep 0.015
@@ -98,38 +129,24 @@ begin
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(GEAR, GEAR_ON)
- sleep 0.010
+ sleep 0.020
check_led 1
check_motors_are_off
- # Check that motors turn on
+ puts("Check that motors turn on")
File.write(THROTTLE, THROTTLE_MID)
- motor1 = []
- motor2 = []
- motor3 = []
- motor4 = []
- for i in 0..100
- 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
- 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})"
+ 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(GEAR, GEAR_OFF)
- sleep 0.015
+ sleep 0.040
check_motors_are_off
check_led 0
@@ -138,7 +155,7 @@ begin
# Check that we can resume flight
File.write(GEAR, GEAR_ON)
- sleep 0.015
+ sleep 0.040
check_led 1
sleep 1
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index 09ab43c8b..9de9815c2 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -1,6 +1,59 @@
#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;
+}
--
GitLab