diff --git a/groundStation/scripts/bypass_kill_switch.sh b/groundStation/scripts/bypass_kill_switch.sh new file mode 100755 index 0000000000000000000000000000000000000000..b1061e271a8bef3effbc2a9e9f7f6acba81d4ab7 --- /dev/null +++ b/groundStation/scripts/bypass_kill_switch.sh @@ -0,0 +1,8 @@ +#! /bin/bash +cd .. +while true ; do + ./setparam 37 0 $1 + sleep 0.4 + ./setparam 37 0 $(( $1 - 1 )) + sleep 0.4 +done diff --git a/groundStation/scripts/parameterize.sh b/groundStation/scripts/parameterize.sh new file mode 100755 index 0000000000000000000000000000000000000000..591656810626bb7867698124adb50f46bc5e6579 --- /dev/null +++ b/groundStation/scripts/parameterize.sh @@ -0,0 +1,10 @@ +cd .. +./addnode 0 "Zero" +./setparam "zero" 0 0 +./setsource "signal mixer" "pitch" "zero" 0 +./setsource "signal mixer" "roll" "zero" 0 +./setsource "signal mixer" "yaw" "zero" 0 +./addnode 0 "PWM_VAL" +./setparam "pwm_val" 0 100000 +./setsource "signal mixer" "throttle" "pwm_val" 0 + diff --git a/quad/executable.mk b/quad/executable.mk index 9a5cce878ecb1e0cc55d209dc6acf3c7ec2e6396..0f5f6435227675076c41ae9cdafe964c1bdec8f6 100644 --- a/quad/executable.mk +++ b/quad/executable.mk @@ -23,7 +23,7 @@ CLEANUP = $(TARGET) $(OBJDIR) default: $(TARGET) run: $(TARGET) - cd $(EXEDIR) && ./$(NAME) + $(EXEDIR)/$(NAME) clean: rm -rf $(CLEANUP) 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 0000000000000000000000000000000000000000..9bc3afa362eb0fcbe4e99d0c6c130c6da5a1fe6a --- /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 2ed03ed7f9f78f3b243aac5937779b1d1123027e..f4e6904955db2d2fd99a220c6c79257e4ed4956a 100644 --- a/quad/scripts/tests/test_safety_checks.rb +++ b/quad/scripts/tests/test_safety_checks.rb @@ -15,26 +15,67 @@ 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" + +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 +$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 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 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 = 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__)) @@ -51,80 +92,70 @@ 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("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 + 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("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 + puts("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 + File.write(GEAR, GEAR_ON) + sleep 0.020 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})" + 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) # 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 + File.write(GEAR, GEAR_OFF) + sleep 0.040 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) - sleep 0.015 + File.write(GEAR, GEAR_ON) + 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 09ab43c8b83b82d60dff7623a31efb1d29c3fd60..9de9815c2f7a941b0b553df525a3436f069ae0c4 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; +} 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 5215423781ca1ebdd0366e95ac4b824c8ec09ce4..6aef66752f9859faf55ec15c200e68440e6b4f7f 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 75805653cc400e8c0707e7d4955fb22a119ef471..b487461095d1a900c16ea2f515454c650d197f3f 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 03f9815cfb3e9dfc1673e7fcda021c9e2b79e44d..71b89c4f4b0d8465780f39a77267301aaafc39cc 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; }