test_safety_checks.rb

#!/usr/bin/env ruby

# Safety Checks
#
# Startup the virtual quad and make sure it doesn't allow combinations of things
# that could hurt people.
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"

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 'timeout'
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 = 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(is_on)
  led = read_fifo_num(LED)
  assert_equal(led, is_on)
end

Timeout::timeout(30) {

  puts("Setting up...")

  script_dir = File.expand_path(File.dirname(__FILE__))
  bin_dir = script_dir + "/../../bin/"
  Dir.chdir(bin_dir)

  sleep 1

  # Start virtual quad
  quad_pid = Process.spawn("./virt-quad", :rlimit_as => 536870912) # 512 MiB RAM

  sleep 5

  #################
  #  Begin Tests
  #################

  begin

    puts("Beginning tests...")

    # Set gravity
    File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)

    puts("Check that motors are off at startup")
    check_motors_are_off

    puts("Check that LED is off at startup")
    check_led(0)

    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)
      check_motors_are_off
      sleep 0.005
    end

    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
    check_motors_are_off
    i = THROTTLE_MAX
    while i > THROTTLE_MID
      i -= 1000
      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(GEAR, GEAR_OFF)
    File.write(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.1
    check_led 1
    check_motors_are_off

    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(GEAR, GEAR_OFF)
    sleep 0.1
    check_motors_are_off
    check_led 0

    # (Bring the RC throttle back down)
    File.write(THROTTLE, THROTTLE_MIN)

    # Check that we can resume flight
    File.write(GEAR, GEAR_ON)
    sleep 0.1
    check_led 1

    sleep 1
    puts "All safety checks passed."

  ensure

    Process.kill(9, quad_pid)
    Process.wait(quad_pid)

  end
}