argus 0.0.0 → 0.3.0

data/lib/argus/nav_streamer.rb

@@ -0,0 +1,111 @@
+require 'socket'
+
+module Argus
+
+  # NavStreamer State Transitions:
+  #
+  # State    Trigger         Next  Action
+  # ------   --------------  ----  -----------------
+  # init     start           wait  reconnect
+  #
+  # wait     short_timeout   wait  request_nav_data
+  # wait     long_timeout    wait  reconnect
+  # wait     received_data   run   --
+  #
+  # run      short_timeout   run   --
+  # run      long_timeout    wait  reconnect
+  # run      received_data   run   --
+
+  class NavStreamer
+    attr_reader :state
+
+    def initialize(opts={})
+      @state = :init
+      @socket = nil
+      @remote_host  = opts.fetch(:remote_host)
+      @local_host   = opts[:local_host]  || '0.0.0.0'
+      @port         = opts[:port]        || 5554
+      @socket_class = opts[:UDPSocket]   || UDPSocket
+      start_timer
+    end
+
+    def start
+      @state = :wait
+      reconnect
+    end
+
+    def short_timeout
+      if state == :wait
+        request_nav_data
+      end
+    end
+
+    def long_timeout
+      if @state == :wait || @state == :run
+        @state = :wait
+        reconnect
+      end
+    end
+
+    def receive_data
+     data, from = @socket.recvfrom(1024)
+      if data.unpack("V").first == 0x55667788
+        received_data
+        NavData.new(data)
+      else
+        nil
+      end
+    end
+
+    def received_data
+      if state == :wait || state == :run
+        @state = :run
+        @long_flag = false
+        @short_flag = false
+      end
+    end
+
+    private
+
+    def request_nav_data
+      @socket.send("\x01\x00\x00\x00", 0, @remote_host, @port)
+    end
+
+    def reconnect
+      disconnect if @socket
+      @socket = new_socket
+      @socket.bind(@local_host, @port) rescue nil
+      request_nav_data
+    end
+
+    def disconnect
+      @socket.close
+      @socket = nil
+    end
+
+    def new_socket
+      @socket_class.new
+    end
+
+    def tick
+      short_timeout if @short_flag
+      @short_flag = true
+      if @n == 0
+        long_timeout if @long_flag
+        @long_flag = true
+      end
+      @n += 1
+      @n = 0 if @n == 10
+    end
+
+    def start_timer
+      @n = 0
+      @thread = Thread.new do
+        loop do
+          tick
+          sleep 1.0
+        end
+      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/null_nav_monitor.rb

@@ -0,0 +1,13 @@
+module Argus
+
+  class NullNavMonitor
+    UsageError = Class.new(StandardError)
+    def callback(*)
+      fail UsageError, "Callbacks are not supported when the NavMonitor is disabled."
+    end
+    def method_missing(*)
+      # Noop
+    end
+  end
+
+end

data/lib/argus/pave_parser.rb

@@ -0,0 +1,39 @@
+module Argus
+  class PaVEParser
+    def initialize(tcp_video_streamer)
+      @tcp_video_streamer = tcp_video_streamer
+    end
+
+    def get_frame
+      frame = {
+        :signature=>(@tcp_video_streamer.read(4)).unpack("A*").first,
+        :version=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :video_codec=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :header_size=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :payload_size=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :encoded_stream_width=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :encoded_stream_height=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :display_width=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :display_height=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :frame_number=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :timestamp=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :total_chunks=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :chunk_index=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :frame_type=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :control=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :stream_byte_position_lw=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :stream_byte_position_uw=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :stream_id=>(@tcp_video_streamer.read(2)).unpack("v").first,
+        :total_slices=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :slice_index=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :header1_size=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :header2_size=>(@tcp_video_streamer.read(1)).unpack("C").first,
+        :reserved[2]=>(@tcp_video_streamer.read(2)).unpack("H*").first,
+        :advertised_size=>(@tcp_video_streamer.read(4)).unpack("V").first,
+        :reserved[12]=>(@tcp_video_streamer.read(12)).unpack("H*").first
+      }
+      @tcp_video_streamer.read(4)
+      return @tcp_video_streamer.read(frame[:payload_size])
+    end
+  end
+end

data/lib/argus/tcp_video_streamer.rb

@@ -0,0 +1,20 @@
+require 'socket'
+module Argus
+  class TcpVideoStreamer
+    def initialize(opts={})
+      @host = opts.fetch(:remote_host)
+      @port = opts.fetch(:port, 5555)
+      @tcp_socket = TCPSocket.new(@host, @port)
+    end
+
+    def start_stream(udp_socket=nil)
+      sock = udp_socket || UDPSocket.new
+      sock.send("\x01\x00\x00\x00", 0, @host, @port)
+      sock.close
+    end
+
+    def read(n)
+      @tcp_socket.read(n)
+    end
+  end
+end

data/lib/argus/time_queue.rb

@@ -0,0 +1,62 @@
+class TimeQueue
+  Item = Struct.new(:time, :value) do
+    def <=>(other)
+      time <=> other.time
+    end
+
+    def key
+      object_id
+    end
+
+    def to_s(basetime=nil)
+      if basetime
+        "#{value.inspect}@+#{time-basetime}"
+      else
+        "#{value.inspect}@#{time.strftime('%H:%M:%S.%L')}"
+      end
+    end
+  end
+
+  def initialize
+    @items = []
+  end
+
+  def add(time, value)
+    item = Item.new(time, value)
+    @items << item
+    @items.sort!
+    item.key
+  end
+
+  def remove(key)
+    @items.delete_if { |item| item.key == key }
+  end
+
+  def any_ready?(time)
+    ! @items.empty? && @items.first.time <= time
+  end
+
+  def all_ready(time)
+    result = []
+    each_ready(time) do |value| result << value end
+    result
+  end
+
+  def each_ready(time)
+    while any_ready?(time)
+      item = @items.shift
+      yield item.value
+    end
+  end
+
+  def to_s
+    base_time = nil
+    strings = @items[0,3].map { |item|
+      string = item.to_s(base_time)
+      base_time ||= item.time
+      string
+    }
+    strings << "..." if @items.size > 3
+    "[#{strings.join(', ')}]"
+  end
+end

data/lib/argus/udp_sender.rb

@@ -0,0 +1,13 @@
+module Argus
+  class UdpSender
+    def initialize(opts={})
+      @udp_socket = opts[:socket] || UDPSocket.new
+      @host = opts.fetch(:remote_host)
+      @port = opts.fetch(:port, 5556)
+    end
+
+    def send_packet(data)
+      @udp_socket.send(data, 0, @host, @port)
+    end
+  end
+end

data/lib/argus/version.rb

@@ -0,0 +1,10 @@
+module Argus
+  module Version # :nodoc: all
+    NUMBERS = [
+      MAJOR = 0,
+      MINOR = 3,
+      BUILD = 0,
+    ]
+  end
+  VERSION = Version::NUMBERS.join('.')
+end

data/spec/argus/at_commander_spec.rb

@@ -0,0 +1,82 @@
+require 'spec_helper'
+
+describe Argus::ATCommander do
+  Given(:sender) { flexmock(:on, Argus::UdpSender) }
+  Given(:cmdr) { Argus::ATCommander.new(sender) }
+
+  describe "#tick" do
+    Given(:expected_commands) {
+      [
+        "AT\\*REF=\\d+,0",
+        "AT\\*PCMD=\\d+,0,0,0,0,0",
+      ]
+    }
+    Given(:command_pattern) { Regexp.new('\A' + expected_commands.map { |s| "#{s}\r" }.join + '\Z') }
+
+    When { cmdr.send(:tick) }
+
+    context "with no commands" do
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+
+    context "with a ref command" do
+      Given { expected_commands[0] = 'AT\*REF=\d+,512' }
+      Given { cmdr.ref("512") }
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+
+    context "with a pcmd command" do
+      Given { expected_commands[1] = 'AT\*PCMD=\d+,1,2,3,4,5' }
+      Given { cmdr.pcmd("1,2,3,4,5") }
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+
+    context "with a config command" do
+      Given { expected_commands.unshift('AT\*CONFIG=\d+,"general:navdata_demo","TRUE"') }
+      Given { cmdr.config("general:navdata_demo", "TRUE") }
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+
+    context "with a reset watchdog command" do
+      Given { expected_commands.unshift('AT\*COMWDG=\d+') }
+      Given { cmdr.comwdg }
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+
+    context "with a contrl command" do
+      Given { expected_commands.unshift('AT\*CTRL=\d+,4,0') }
+      Given { cmdr.ctrl(4) }
+      Then { sender.should have_received(:send_packet).with(command_pattern) }
+    end
+  end
+
+  describe "increasing sequence numbers" do
+    Given(:seq_numbers) { [ ] }
+    Given {
+      sender.should_receive(:send_packet).and_return { |data|
+        data.scan(%r{=(\d+),}) { |seq|
+          seq_numbers << seq.first.to_i }
+      }
+    }
+    When { 2.times { cmdr.tick } }
+    Then { seq_numbers.should == [1, 2, 3, 4] }
+  end
+
+  describe "sending thread" do
+    Given {
+      @count = 0
+      sender.should_receive(:send_packet).and_return {
+        @count += 1
+        cmdr.stop if @count > 5
+      }
+    }
+    When {
+      cmdr.start
+      cmdr.join
+    }
+    Then { sender.should have_received(:send_packet) }
+  end
+
+
+
+end

data/spec/argus/controller_spec.rb

@@ -0,0 +1,149 @@
+require 'spec_helper'
+
+describe Argus::Controller do
+
+  REF_BASE = 0x11540000
+
+  def ref_bits(n=nil)
+    if n.nil?
+      REF_BASE.to_s
+    else
+      (REF_BASE | (1 << n)).to_s
+    end
+  end
+
+  Given(:at) { flexmock(:on, Argus::ATCommander) }
+  Given(:controller) { Argus::Controller.new(at) }
+
+  context "navigating commands" do
+    Invariant { result.should == controller }
+
+    context "when taking off" do
+      When(:result) { controller.take_off }
+      Then { at.should have_received(:ref).with(ref_bits(9)) }
+    end
+
+    context "when landing" do
+      When(:result) { controller.land }
+      Then { at.should have_received(:ref).with(ref_bits).twice }
+    end
+
+    context "when emergency" do
+      Given { controller.take_off }
+      When(:result) { controller.emergency }
+      Then { at.should have_received(:ref).with(ref_bits(8)) }
+    end
+
+    context "when taking off after an emergency" do
+      Given { controller.emergency }
+      When(:result) { controller.take_off }
+      Then { at.should have_received(:ref).with(ref_bits(9)) }
+    end
+
+    context "when hovering" do
+      When(:result) { controller.hover }
+      Then { at.should have_received(:pcmd).with("0,0,0,0,0") }
+    end
+
+    context "when moving forward" do
+      When(:result) { controller.forward(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,-1085485875,0,0") }
+    end
+
+    context "when moving backward" do
+      When(:result) { controller.backward(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,1061997773,0,0") }
+    end
+
+    context "when moving to the left" do
+      When(:result) { controller.left(0.80) }
+      Then { at.should have_received(:pcmd).with("1,-1085485875,0,0,0") }
+    end
+
+    context "when moving to the right" do
+      When(:result) { controller.right(0.80) }
+      Then { at.should have_received(:pcmd).with("1,1061997773,0,0,0") }
+    end
+
+    context "when moving up" do
+      When(:result) { controller.up(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,0,1061997773,0") }
+    end
+
+    context "when moving down" do
+      When(:result) { controller.down(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,0,-1085485875,0") }
+    end
+
+    context "when turning left" do
+      When(:result) { controller.turn_left(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,0,0,-1085485875") }
+    end
+
+    context "when turning right" do
+      When(:result) { controller.turn_right(0.80) }
+      Then { at.should have_received(:pcmd).with("1,0,0,0,1061997773") }
+    end
+
+    context "when executing several directions" do
+      When(:result) {
+        controller.forward(1.0).left(0.5).up(0.2).turn_left(0.8)
+      }
+      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
+
+  describe "ack controll command" do
+    Invariant { result.should == controller }
+    When(:result) { controller.ack_control_mode }
+    Then { at.should have_received(:ctrl).with(5) }
+  end
+end