Quad i2c rework

Problem

Currently, the I2C device specific functions are residing in the application layer. This doesn't make much sense; it would be more appropriate for these hardware level details (registers numbers and offsets) to reside in the hardware layer.

Solution

Add a few more hardware interfaces, specifically one for each of the I2C sensors we use (IMU, Lidar, Optical flow). This will make dependency injection of the I2C bus on a particular bus much easier (which will be important if we want to make a new I2C bus specifically for the optical flow sensor). As a bonus, this change also makes it much easier to implement the I2C devices in the virtual quad, and allows us to make some cool tests (like verifying that certain motors get stronger when you "tilt" the quad...).

Reviewers

@dawehr
@ericm @jpbush @phjones

quad/scripts/tests/test_safety_checks.rb

@@ -12,7 +12,7 @@ bin_dir = script_dir + "/../../bin/"
 Dir.chdir(bin_dir)
 
 
-Timeout::timeout(30) {
+Timeout::timeout(60) {
 
   puts("Setting up...")
 
@@ -23,21 +23,25 @@ Timeout::timeout(30) {
 
   delay_spin_cursor(1)
 
+  # Set RC switches
+  set_gear GEAR_OFF
+  set_flap FLAP_ON
+
+  # Set initial quad orientation (flat on the ground, facing forward)
+  `./virt-quad set i2c_imu_x 0`
+  `./virt-quad set i2c_imu_y 0`
+  `./virt-quad set i2c_imu_z -1`
+  `./virt-quad set rc_roll 0.498`
+  `./virt-quad set rc_pitch 0.497`
+  `./virt-quad set rc_yaw 0.498`
+
   #################
   #  Begin Tests
   #################
 
   begin
-
     puts("Beginning tests...")
 
-    # Set gravity and gear
-    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!"
 
@@ -80,12 +84,53 @@ Timeout::timeout(30) {
     puts("Check that motors turn on")
     set_throttle THROTTLE_MID
     spinner = Thread.new { delay_spin_cursor(5) }
-    motors = get_motor_averages(100, 5)
+    motors = get_motor_averages(100, 3)
+    spinner.exit
+    p motors
+    motors.each { |value| assert_operator(value, :>, THROTTLE_QUAR) }
+
+    puts("Check that when quad is tilted, motors respond correctly")
+    puts("Tilting forwards...")
+    `./virt-quad set i2c_imu_x 0.25`
+    spinner = Thread.new { delay_spin_cursor(5) }
+    motors = get_motor_averages(100, 3)
+    spinner.exit
+    p motors
+    assert_operator(motors[0], :>, motors[1])
+    assert_operator(motors[0], :>, motors[3])
+    assert_operator(motors[2], :>, motors[1])
+    assert_operator(motors[2], :>, motors[3])
+    puts("Tilting backwards...")
+    `./virt-quad set i2c_imu_x -0.25`
+    spinner = Thread.new { delay_spin_cursor(5) }
+    motors = get_motor_averages(100, 3)
+    spinner.exit
+    p motors
+    assert_operator(motors[0], :<, motors[1])
+    assert_operator(motors[0], :<, motors[3])
+    assert_operator(motors[2], :<, motors[1])
+    assert_operator(motors[2], :<, motors[3])
+    puts("Tilting right...")
+    `./virt-quad set i2c_imu_x 0`
+    `./virt-quad set i2c_imu_y 0.25`
+    spinner = Thread.new { delay_spin_cursor(5) }
+    motors = get_motor_averages(100, 3)
+    spinner.exit
+    p motors
+    assert_operator(motors[0], :<, motors[2])
+    assert_operator(motors[0], :<, motors[3])
+    assert_operator(motors[1], :<, motors[2])
+    assert_operator(motors[1], :<, motors[3])
+    puts("Tilting left...")
+    `./virt-quad set i2c_imu_y -0.25`
+    spinner = Thread.new { delay_spin_cursor(5) }
+    motors = get_motor_averages(100, 3)
     spinner.exit
     p motors
-    # motors.each { |value| assert_operator(value, :>, THROTTLE_EIGHTH) }
-    motors_average = motors.reduce(:+).to_f / 4
-    assert_operator(motors_average, :>, THROTTLE_EIGHTH)
+    assert_operator(motors[0], :>, motors[2])
+    assert_operator(motors[0], :>, motors[3])
+    assert_operator(motors[1], :>, motors[2])
+    assert_operator(motors[1], :>, motors[3])
 
     puts("Check that gear switch kills the motors")
     set_gear GEAR_OFF