hybridgroup-argus 0.2.0 → 0.3.0

data/lib/argus/nav_option_demo.rb

@@ -0,0 +1,79 @@
+require 'argus/cfields'
+require 'argus/float_encoding'
+require 'argus/nav_option'
+require 'argus/nav_tag'
+
+module Argus
+
+  class NavOptionDemo < NavOption
+    include CFields
+
+    uint32_t :ctrl_state               # Flying state (landed, flying,
+    alias :control_state :ctrl_state
+                                       # hovering, etc.) defined in
+                                       # CTRL_STATES enum.
+    uint32_t :vbat_flying_percentage   # battery voltage filtered (mV)
+    alias :battery_level :vbat_flying_percentage
+
+    float32_t :theta                   # UAV's pitch in milli-degrees
+    float32_t :phi                     # UAV's roll  in milli-degrees
+    float32_t :psi                     # UAV's yaw   in milli-degrees
+
+    int32_t :altitude                  # UAV's altitude in centimeters
+
+    float32_t :vx                      # UAV's estimated linear velocity
+    float32_t :vy                      # UAV's estimated linear velocity
+    float32_t :vz                      # UAV's estimated linear velocity
+
+    uint32_t :num_frames               # streamed frame index // Not
+                                       # used -> To integrate in video
+                                       # stage.
+
+    # Camera parameters compute by detection
+    matrix33_t :detection_camera_rot   # Deprecated ! Don't use !
+    vector31_t :detection_camera_trans # Deprecated ! Don't use !
+    uint32_t :detection_tag_index      # Deprecated ! Don't use !
+
+    uint32_t :detection_camera_type    #  Type of tag searched in detection
+
+    # Camera parameters compute by drone
+    matrix33_t :drone_camera_rot       # Deprecated ! Don't use !
+    vector31_t :drone_camera_trans     # Deprecated ! Don't use !
+
+    CONTROL_STATE_NAMES = [
+      :default,
+      :init,
+      :landed,
+      :flying,
+      :hovering,
+      :test,
+      :trans_takeoff,
+      :trans_gotofix,
+      :trans_landing,
+      :trans_looping,
+    ]
+
+    def control_state_name
+      CONTROL_STATE_NAMES[ctrl_state]
+    end
+
+    def pitch
+      theta / 1000.0
+    end
+
+    def roll
+      phi / 1000.0
+    end
+
+    def yaw
+      psi / 1000.0
+    end
+
+    def self.tag
+      NavTag::DEMO
+    end
+
+    NavOption.register(self)
+  end
+
+end

data/lib/argus/nav_option_unknown.rb

@@ -0,0 +1,9 @@
+module Argus
+
+  class NavOptionUnknown < NavOption
+    def self.tag
+      NavTag::UNKNOWN
+    end
+  end
+
+end

data/lib/argus/nav_option_vision_detect.rb

@@ -0,0 +1,83 @@
+require 'argus/cfields'
+require 'argus/nav_tag'
+
+module Argus
+
+  class NavOptionVisionDetect < NavOption
+    include CFields
+
+    NB_NAVDATA_DETECTION_RESULTS = 4
+
+    uint32_t   :nb_detected
+    alias :detected_count :nb_detected
+    uint32_t   :type, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :xc, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :yc, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :width, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :height, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :dist, NB_NAVDATA_DETECTION_RESULTS
+    alias :distance :dist
+    float32_t  :orientation_angle, NB_NAVDATA_DETECTION_RESULTS
+    matrix33_t :rotation, NB_NAVDATA_DETECTION_RESULTS
+    vector31_t :translation, NB_NAVDATA_DETECTION_RESULTS
+    uint32_t   :camera_source, NB_NAVDATA_DETECTION_RESULTS
+
+    class Detection
+      def initialize(vision_detected, index)
+        @vision_detected = vision_detected
+        @index = index
+      end
+
+      def type
+        @vision_detected.type[@index]
+      end
+
+      def type_name
+        CadType::NAMES[type]
+      end
+
+      def x
+        @vision_detected.xc[@index]
+      end
+
+      def y
+        @vision_detected.yc[@index]
+      end
+
+      def width
+        @vision_detected.width[@index]
+      end
+
+      def height
+        @vision_detected.height[@index]
+      end
+
+      def distance
+        @vision_detected.distance[@index]
+      end
+
+      def orientation_angle
+        @vision_detected.orientation_angle[@index]
+      end
+
+      def camera_source
+        @vision_detected.camera_source[@index]
+      end
+    end
+
+    def type_name
+      type.map { |t| CadType::NAMES[t] }
+    end
+
+    def detections
+      @detections ||= (0...detected_count).map { |i| Detection.new(self, i) }
+    end
+
+    def self.tag
+      NavTag::VISION_DETECT
+    end
+
+    NavOption.register(self)
+  end
+
+end

data/lib/argus/nav_streamer.rb

@@ -1,18 +1,26 @@
 require 'socket'
 
 module Argus
-  class NavStreamer 
+  class NavStreamer
     def initialize(socket=nil, host='192.168.1.1', port='5554')
+      # TODO: Why is the port a string?
       @socket = socket || UDPSocket.new
-      @streamer = Argus::UdpNavStreamer.new(@socket, host, port)
+      @host = host
+      @port = port
+      @socket.bind("0.0.0.0", port)
     end
 
     def start
-      @streamer.start_stream
+      @socket.send("\x01\x00\x00\x00", 0, @host, @port)
     end
 
     def receive_data
-      @streamer.receive_packet
+      data, from = @socket.recvfrom(1024)
+      if data.unpack("V").first == 0x55667788
+        NavData.new(data)
+      else
+        nil
+      end
     end
   end
 end

data/lib/argus/nav_tag.rb

@@ -0,0 +1,35 @@
+module Argus
+  module NavTag
+    DEMO,
+    TIME,
+    RAW_MEASURES,
+    PHYS_MEASURES,
+    GYROS_OFFSETS,
+    EULER_ANGLES,
+    REFERENCES,
+    TRIMS,
+    RC_REFERENCES,
+    PWM,
+    ALTITUDE,
+    VISION_RAW,
+    VISION_OF,
+    VISION,
+    VISION_PERF,
+    TRACKERS_SEND,
+    VISION_DETECT,
+    WATCHDOG,
+    ADC_DATA_FRAME,
+    VIDEO_STREAM,
+    GAMES,
+    PRESSURE_RAW,
+    MAGNETO,
+    WIND,
+    KALMAN_PRESSURE,
+    HDVIDEO_STREAM,
+    WIFI = (0..100).to_a
+
+    CHECKSUM = 0xffff
+    UNKNOWN  = 0xfffe           # Not part of the AR Drone DSK, Only used in Argus
+  end
+
+end

data/lib/argus/version.rb

@@ -2,7 +2,7 @@ module Argus
   module Version # :nodoc: all
     NUMBERS = [
       MAJOR = 0,
-      MINOR = 2,
+      MINOR = 3,
       BUILD = 0,
     ]
   end

data/lib/argus/video_data.rb

@@ -0,0 +1,15 @@
+module Argus
+  class VideoData
+    attr_reader :socket, :envelope, :frame
+
+    def initialize(socket)
+      @socket = socket
+      @envelope = VideoDataEnvelope.new(@socket)
+      @frame = parse_frame
+    end
+
+    def parse_frame
+      socket.read(envelope.payload_size) if envelope
+    end
+  end
+end

data/lib/argus/video_data_envelope.rb

@@ -0,0 +1,55 @@
+module Argus
+  class VideoDataEnvelope
+    attr_reader :socket
+
+    def initialize(socket)
+      @socket = socket
+      parse
+    end
+
+    def parse
+      data(:signature, 4, "A*")
+      data(:version, 1, "C")
+      data(:video_codec, 1, "C")
+      data(:header_size, 2, "v")
+      data(:payload_size, 4, "V")
+      data(:encoded_stream_width, 2, "v")
+      data(:encoded_stream_height, 2, "v")
+      data(:display_width, 2, "v")
+      data(:display_height, 2, "v")
+      data(:frame_number, 4, "V")
+      data(:timestamp, 4, "V")
+      data(:total_chunks, 1, "C")
+      data(:chunk_index, 1, "C")
+      data(:frame_type, 1, "C")
+      data(:control, 1, "C")
+      data(:stream_byte_position_lw, 4, "V")
+      data(:stream_byte_position_uw, 4, "V")
+      data(:stream_id, 2, "v")
+      data(:total_slices, 1, "C")
+      data(:slice_index, 1, "C")
+      data(:header1_size, 1, "C")
+      data(:header2_size, 1, "C")
+      data(:reserved_2, 2, "H*")
+      data(:advertised_size, 4, "V")
+      data(:reserved_12, 12, "H*")
+      socket.read(4) # TODO: look at why just throwing this data away
+    end
+
+    def data(name, size, format)
+      define_data(name, get_data(size, format))
+    end
+
+    def get_data(size, format)
+      socket.read(size).unpack(format).first
+    end
+
+    def define_data(name, value)
+      ivar = "@#{name}".to_sym    
+      instance_variable_set(ivar, value)
+      self.class.send(:define_method, name) do
+        instance_variable_get(ivar)
+      end
+    end
+  end
+end

data/lib/argus/video_streamer.rb

@@ -0,0 +1,23 @@
+require 'socket'
+
+module Argus
+  class VideoStreamer
+    attr_reader :tcp_socket, :host, :port
+    
+    def initialize(tcp_socket=nil, host='192.168.1.1', port=5555)
+      @tcp_socket = tcp_socket || TCPSocket.new(host, port)
+      @host = host
+      @port = port
+    end
+
+    def start(udp_socket=nil)
+      @udp_socket = udp_socket || UDPSocket.new
+      @udp_socket.send("\x01\x00\x00\x00", 0, host, port)
+      @udp_socket.close
+    end
+
+    def receive_data
+      VideoData.new(tcp_socket)
+    end
+  end
+end

data/lib/argus.rb

@@ -1,9 +1,5 @@
-require 'argus/at_commander'
-require 'argus/controller'
-require 'argus/udp_sender'
-require 'argus/udp_nav_streamer'
-require 'argus/nav_data'
-require 'argus/nav_streamer'
-require 'argus/drone'
-require 'argus/tcp_video_streamer'
-require 'argus/pave_parser'
+# Require all the top level files in argus/*.rb
+
+Dir[File.dirname(__FILE__) + "/argus/*.rb"].sort.each do |pn|
+  require "argus/#{File.basename(pn, '.rb')}"
+end

data/spec/argus/controller_spec.rb

@@ -92,4 +92,53 @@ describe Argus::Controller do
       Then { at.should have_received(:pcmd).with("1,-1090519040,-1082130432,1045220557,-1085485875") }
     end
   end
+
+  describe "led command" do
+    Invariant { result.should == controller }
+
+    context "when setting with numeric sequence" do
+      When(:result) { controller.led(3, 2.0, 4) }
+      Then {
+        at.should have_received(:config)
+          .with("leds:leds_anim", "3,1073741824,4")
+      }
+    end
+
+    context "when setting with symbolic sequence" do
+      When(:result) { controller.led(:blink_orange, 2.0, 4) }
+      Then {
+        at.should have_received(:config)
+          .with("leds:leds_anim", "3,1073741824,4")
+      }
+    end
+  end
+
+  describe "enabled detection command" do
+    Invariant { result.should == controller }
+
+    context "when setting all parameters" do
+      Given(:enemy)  { 2 }
+      Given(:type)   { 11 }
+      Given(:select) { 33 }
+
+      When(:result) { controller.enable_detection(enemy, type, select) }
+
+      Then { at.should have_received(:config).with("detect:enemy_colors", enemy.to_s) }
+      Then { at.should have_received(:config).with("detect:detect_type", type.to_s) }
+      Then { at.should have_received(:config).with("detect:detections_select_h", select.to_s) }
+    end
+
+    context "when setting with only enemy" do
+      Given(:enemy)  { 2 }
+      Given(:type)   { 10 }
+      Given(:select) { 32 }
+
+      When(:result) { controller.enable_detection(enemy) }
+
+      Then { at.should have_received(:config).with("detect:enemy_colors", enemy.to_s) }
+      Then { at.should have_received(:config).with("detect:detect_type", type.to_s) }
+      Then { at.should have_received(:config).with("detect:detections_select_h", select.to_s) }
+    end
+  end
+
 end

data/spec/argus/drone_spec.rb

@@ -0,0 +1,24 @@
+require 'spec_helper'
+
+module Argus
+  describe Drone do
+
+    Given(:socket) { flexmock("Socket", send: nil).should_ignore_missing }
+    Given(:drone) { Drone.new(socket) }
+
+    describe "default navigation monitor enabled" do
+      Then { drone.enable_nav_monitor == false }
+    end
+
+    describe "default navigation monitor enabled when started" do
+      When { drone.start }
+      Then { drone.enable_nav_monitor == true }
+    end
+
+    describe "navigation monitor can be disabled when started" do
+      When { drone.start(false) }
+      Then { drone.enable_nav_monitor == false }
+    end
+  end
+end
+

data/spec/argus/float_encoding_spec.rb

@@ -0,0 +1,12 @@
+require 'spec_helper'
+
+module Argus
+
+  describe FloatEncoding do
+    Given(:f) { 5.0 }
+    Given(:encoded_f) { 1084227584 }
+    Then { FloatEncoding.encode_float(f) == encoded_f }
+    Then { FloatEncoding.decode_float(encoded_f) == f }
+  end
+
+end

data/spec/argus/led_animation_spec.rb

@@ -0,0 +1,21 @@
+require 'spec_helper'
+
+module Argus
+
+  describe LedAnimation do
+    describe ".lookup_name" do
+      Then { LedAnimation.lookup_name(3) == :blink_orange }
+    end
+
+    describe ".lookup_value" do
+      Then { LedAnimation.lookup_value(:blink_orange) == 3 }
+      Then { LedAnimation.lookup_value("blink_orange") == 3 }
+      Then { LedAnimation.lookup_value(3) == 3 }
+      Then { LedAnimation.lookup_value("3") == 3 }
+
+      Then { LedAnimation.lookup_value(:unknown) == nil }
+      Then { LedAnimation.lookup_value("UNKNOWN") == nil }
+    end
+  end
+
+end

data/spec/argus/nav_data_spec.rb

@@ -0,0 +1,147 @@
+require 'spec_helper'
+
+module Argus
+  describe NavData do
+    Given(:state_bits) { 0xcf8a0c94 }
+    Given(:seq_num) { 173064 }
+    Given(:vision_flag) { 0 }
+    Given(:raw_header) { Bytes.make_header(state_bits, seq_num, vision_flag) }
+    Given(:raw_nav_bytes) { Bytes.make_nav_data(raw_header) }
+
+    When(:nav_data) { NavData.new(raw_nav_bytes) }
+
+    Then { nav_data.sequence_number == 173064 }
+    Then { nav_data.vision_flag == 0 }
+
+    describe "sequence number" do
+      Given(:seq_num) { 1234 }
+      Then { nav_data.sequence_number == 1234 }
+    end
+
+    describe "vision flag" do
+      Given(:vision_flag) { 1 }
+      Then { nav_data.vision_flag == 1 }
+    end
+
+    describe "state queries" do
+
+      def bit(n)
+        0x00000001 << n
+      end
+
+      # Matcher for handling the mask testing for the state mask.
+      #
+      # Usage:
+      #    bit(5).should be_the_mask_for(:method [, off_value, on_value])
+      #
+      # If off_value and on_value are not given, then false/true will
+      # be assumed.
+      #
+      matcher :be_the_mask_for do |method, off_result=false, on_result=true|
+        match do |mask|
+          @msg = "OK"
+          try_alternative(method, 0, off_result) &&
+            try_alternative(method, mask, on_result)
+        end
+
+        failure_message_for_should do |mask|
+          @msg
+        end
+
+        # Try one of the alternatives. Sending method to the navdata
+        # constructed with mask should return teh expected value.
+        def try_alternative(method, mask, expected)
+          raw = Bytes.make_nav_data(Bytes.make_header(mask, 1, 0))
+          nav_data = NavData.new(raw)
+          result = nav_data.send(method)
+          return true if result == expected
+          @msg = "expected mask of 0x#{'%08x' % mask} with #{method}\n" +
+            "to return: #{expected.inspect}\n" +
+            "got:       #{result.inspect}"
+          false
+        end
+      end
+
+      Then { bit(0).should be_the_mask_for(:flying?) }
+      Then { bit(1).should be_the_mask_for(:video_enabled?) }
+      Then { bit(2).should be_the_mask_for(:vision_enabled?) }
+      Then { bit(3).should be_the_mask_for(:control_algorithm, :euler, :angular_speed) }
+
+      Then { bit(4).should be_the_mask_for(:altitude_control_algorithm_active?) }
+      Then { bit(5).should be_the_mask_for(:start_button_pressed?) }
+      Then { bit(6).should be_the_mask_for(:control_command_ack?) }
+      Then { bit(7).should be_the_mask_for(:camera_ready?) }
+
+      Then { bit(8).should be_the_mask_for(:travelling_enabled?) }
+      Then { bit(9).should be_the_mask_for(:usb_ready?) }
+      Then { bit(10).should be_the_mask_for(:mode, :all, :demo) }
+      Then { bit(11).should be_the_mask_for(:bootstrap?) }
+
+      Then { bit(12).should be_the_mask_for(:moter_problem?) }
+      Then { bit(13).should be_the_mask_for(:communication_lost?) }
+      Then { bit(14).should be_the_mask_for(:software_fault?) }
+      Then { bit(15).should be_the_mask_for(:low_battery?) }
+
+      Then { bit(16).should be_the_mask_for(:emergency_landing_requested?) }
+      Then { bit(17).should be_the_mask_for(:timer_elapsed?) }
+      Then { bit(18).should be_the_mask_for(:magnometer_needs_calibration?) }
+      Then { bit(19).should be_the_mask_for(:angles_out_of_range?) }
+
+      Then { bit(20).should be_the_mask_for(:too_much_wind?) }
+      Then { bit(21).should be_the_mask_for(:ultrasonic_sensor_deaf?) }
+      Then { bit(22).should be_the_mask_for(:cutout_detected?) }
+      Then { bit(23).should be_the_mask_for(:pic_version_number_ok?) }
+
+      Then { bit(24).should be_the_mask_for(:at_codec_thread_on?) }
+      Then { bit(25).should be_the_mask_for(:navdata_thread_on?) }
+      Then { bit(26).should be_the_mask_for(:video_thread_on?) }
+      Then { bit(27).should be_the_mask_for(:acquisition_thread_on?) }
+
+      Then { bit(28).should be_the_mask_for(:control_watchdog_delayed?) }
+      Then { bit(29).should be_the_mask_for(:adc_watchdog_delayed?) }
+      Then { bit(30).should be_the_mask_for(:com_watchdog_problem?) }
+      Then { bit(31).should be_the_mask_for(:emergency_landing?) }
+    end
+
+    describe "state mask" do
+      context "all zeros" do
+        Given(:state_bits) { 0 }
+        Then { nav_data.state_mask == state_bits }
+      end
+
+      context "non-zero" do
+        Given(:state_bits) { 12345 }
+        Then { nav_data.state_mask == state_bits }
+      end
+    end
+
+    describe "#options" do
+      Then { nav_data.options.size == 1 }
+      Then { nav_data.options.first.is_a?(NavOptionChecksum) }
+    end
+  end
+
+  describe "multiple options" do
+    context "with good data" do
+      Given(:raw_header) { Bytes.make_header(0, 0, 0) }
+      Given(:raw_nav_bytes) {
+        Bytes.make_nav_data(raw_header, Bytes.make_demo_data)
+      }
+      When(:nav_data) { NavData.new(raw_nav_bytes) }
+      Then { nav_data.options.size == 2 }
+      Then { nav_data.options[0].is_a?(NavOptionDemo) }
+      Then { nav_data.options[1].is_a?(NavOptionChecksum) }
+    end
+
+    context "with short data" do
+      Given(:raw_nav_bytes) {
+        Bytes.make_header(0, 0, 0).pack("C*") +
+        [4321, 100, 0].pack("vvV")
+      }
+      When(:nav_data) { NavData.new(raw_nav_bytes) }
+      Then { nav_data.options.size == 1 }
+      Then { nav_data.options[0].is_a?(NavOptionUnknown) }
+    end
+  end
+
+end

data/spec/argus/nav_option_checksum_spec.rb

@@ -0,0 +1,21 @@
+require 'spec_helper'
+
+module Argus
+
+  describe NavOptionChecksum do
+    Given(:raw_data) { [NavTag::CHECKSUM, 8, 0x12345678].pack("vvV") }
+    Given(:csum) { NavOptionChecksum.new(raw_data) }
+
+    describe ".tag" do
+      Then { NavOptionChecksum.tag == NavTag::CHECKSUM }
+    end
+
+    describe "data fields" do
+      Then { csum.tag == NavOptionChecksum.tag }
+      Then { csum.size == raw_data.size }
+      Then { csum.chks == 0x12345678 }
+      Then { csum.checksum == 0x12345678 }
+    end
+  end
+
+end

data/spec/argus/nav_option_demo_spec.rb

@@ -0,0 +1,44 @@
+require 'spec_helper'
+
+module Argus
+  describe NavOptionDemo do
+    def f(float)
+      FloatEncoding.encode_float(float)
+    end
+
+    Given(:raw_data) { [
+        NavTag::DEMO, 148,
+        3, 0xcacacaca,
+        f(5.0), f(6.0), f(7.0),
+        -100000,
+        f(-5.0), f(-6.0), f(-7.0),
+        33,
+        [0]*9, [0]*3,
+        12, 34,
+        [0]*9, [0]*3,
+      ].flatten.pack("vv VV V3 l< V3 V V9 V3 VV V9 v3") }
+
+    When(:demo) { NavOptionDemo.new(raw_data) }
+
+    Then { ! demo.nil? }
+
+    Then { demo.ctrl_state == 3 }
+    Then { demo.control_state_name == :flying }
+    Then { demo.vbat_flying_percentage == 0xcacacaca }
+    Then { demo.battery_level == 0xcacacaca }
+    Then { demo.theta == 5.0 }
+    Then { demo.phi == 6.0 }
+    Then { demo.psi == 7.0 }
+    Then { demo.altitude == -100000 }
+    Then { demo.vx == -5.0 }
+    Then { demo.vy == -6.0 }
+    Then { demo.vz == -7.0 }
+    Then { demo.detection_camera_rot.nil? }
+    Then { demo.detection_camera_trans.nil? }
+    Then { demo.detection_tag_index == 12 }
+    Then { demo.detection_camera_type == 34 }
+    Then { demo.drone_camera_rot.nil? }
+    Then { demo.drone_camera_trans.nil? }
+  end
+
+end