argus 0.0.0 → 0.3.0

data/lib/argus/drone.rb

@@ -0,0 +1,62 @@
+require 'socket'
+require 'argus/nav_monitor'
+require 'argus/null_nav_monitor'
+
+module Argus
+  class Drone
+    attr_reader :controller, :nav
+
+    def initialize(opts={})
+      @host = opts[:remote_host] || '192.168.1.1'
+      port = opts[:post] || '5556'
+      @sender = opts[:sender] || Argus::UdpSender.new(socket: opts[:socket], remote_host: @host, port: port)
+      @at = Argus::ATCommander.new(@sender)
+      @controller = Argus::Controller.new(@at)
+      if opts.fetch(:enable_nav_monitor, true)
+        @nav = NavMonitor.new(@controller, @host)
+      else
+        @nav = NullNavMonitor.new
+      end
+    end
+
+    def commander
+      @at
+    end
+
+    def start(enable_nav_monitor=true)
+      @nav.start
+      @at.start
+    end
+
+    def stop
+      @controller.land
+
+      @at.stop
+      @nav.stop
+
+      @at.join
+      @nav.join
+    end
+
+    def nav_callback(*args, &block)
+      @nav.callback(*args, &block)
+    end
+
+    %w(
+       take_off land hover emergency
+       forward backward
+       left right
+       up down
+       turn_left turn_right
+       front_camera bottom_camera
+       config
+       led
+       reset_watchdog
+    ).each do |meth|
+      define_method(meth) { |*args|
+        @controller.send(meth, *args)
+      }
+    end
+
+  end
+end

data/lib/argus/float_encoding.rb

@@ -0,0 +1,13 @@
+module Argus
+  module FloatEncoding
+    module_function
+
+    def encode_float(float)
+      [float].pack('g').unpack("l>").first
+    end
+
+    def decode_float(encoded_float)
+      [encoded_float].pack("l>").unpack("g").first
+    end
+  end
+end

data/lib/argus/led_animation.rb

@@ -0,0 +1,48 @@
+module Argus
+
+  module LedAnimation
+    NAMES = [
+      :blink_green_red,
+      :blink_green,
+      :blink_red,
+      :blink_orange,
+      :snake_green_red,
+      :fire,
+      :standard,
+      :red,
+      :green,
+      :red_snake,
+      :blank,
+      :right_missile,
+      :left_missile,
+      :double_missile,
+      :front_left_green_others_red,
+      :front_right_green_others_red,
+      :rear_right_green_others_red,
+      :rear_left_green_others_red,
+      :left_green_right_red,
+      :left_red_right_green,
+      :blink_standard,
+    ]
+
+    VALUES = {}
+    NAMES.each.with_index { |name, index| VALUES[name] = index }
+
+    def self.lookup_name(numeric_value)
+      NAMES[numeric_value]
+    end
+
+    def self.lookup_value(symbolic_name)
+      case symbolic_name
+      when Symbol
+        VALUES[symbolic_name]
+      when Integer
+        symbolic_name
+      when /^\d+/
+        symbolic_name.to_i
+      when String
+        VALUES[symbolic_name.intern]
+      end
+    end
+  end
+end

data/lib/argus/nav_data.rb

@@ -0,0 +1,78 @@
+module Argus
+  class NavData
+    attr_accessor :state_mask, :vision_flag, :sequence_number, :options
+
+    def initialize(data)
+      @data = data
+      parse_nav_data(@data)
+    end
+
+    def raw
+      @data
+    end
+
+    def self.bit_mask_readers(*names)
+      names.each.with_index do |name, bit_number|
+        if name.is_a?(Array)
+          name, off_value, on_value = name
+        else
+          off_value = false
+          on_value = true
+        end
+        define_method(name) { @state_mask[bit_number] == 0 ? off_value : on_value }
+      end
+    end
+
+    bit_mask_readers(
+      :flying?,                            # FLY MASK : (0) ardrone is landed, (1) ardrone is flying
+      :video_enabled?,                     # VIDEO MASK : (0) video disable, (1) video enable
+      :vision_enabled?,                    # VISION MASK : (0) vision disable, (1) vision enable
+      [:control_algorithm,                 # CONTROL ALGO : (0) euler angles control, (1) angular speed control
+        :euler, :angular_speed],
+      :altitude_control_algorithm_active?, # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active
+      :start_button_pressed?,              # USER feedback : Start button state
+      :control_command_ack?,               # Control command ACK : (0) None, (1) one received
+      :camera_ready?,                      # CAMERA MASK : (0) camera not ready, (1) Camera ready
+      :travelling_enabled?,                # Travelling mask : (0) disable, (1) enable
+      :usb_ready?,                         # USB key : (0) usb key not ready, (1) usb key ready
+      [:mode, :all, :demo],                # Navdata demo : (0) All navdata, (1) only navdata demo
+      :bootstrap?,                         # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent
+      :moter_problem?,                     # Motors status : (0) Ok, (1) Motors problem
+      :communication_lost?,                # Communication Lost : (1) com problem, (0) Com is ok
+      :software_fault?,                    # Software fault detected - user should land as quick as possible (1)
+      :low_battery?,                       # VBat low : (1) too low, (0) Ok
+      :emergency_landing_requested?,       # User Emergency Landing : (1) User EL is ON, (0) User EL is OF
+      :timer_elapsed?,                     # Timer elapsed : (1) elapsed, (0) not elapsed
+      :magnometer_needs_calibration?,      # Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed
+      :angles_out_of_range?,               # Angles : (0) Ok, (1) out of range
+      :too_much_wind?,                     # WIND MASK: (0) ok, (1) Too much wind
+      :ultrasonic_sensor_deaf?,            # Ultrasonic sensor : (0) Ok, (1) deaf
+      :cutout_detected?,                   # Cutout system detection : (0) Not detected, (1) detected
+      :pic_version_number_ok?,             # PIC Version number OK : (0) a bad version number, (1) version number is OK
+      :at_codec_thread_on?,                # ATCodec thread ON : (0) thread OFF (1) thread ON
+      :navdata_thread_on?,                 # Navdata thread ON : (0) thread OFF (1) thread ON
+      :video_thread_on?,                   # Video thread ON : (0) thread OFF (1) thread ON
+      :acquisition_thread_on?,             # Acquisition thread ON : (0) thread OFF (1) thread ON
+      :control_watchdog_delayed?,          # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled
+      :adc_watchdog_delayed?,              # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good
+      :com_watchdog_problem?,              # Communication Watchdog : (1) com problem, (0) Com is ok
+      :emergency_landing?)                 # Emergency landing : (0) no emergency, (1) emergency
+
+    private
+
+    def parse_nav_data(data)
+      tag, @state_mask, @sequence_number, @vision_flag = data.unpack("VVVV")
+      parse_nav_options(data[16..-1])
+    end
+
+    def parse_nav_options(data)
+      @options = []
+      loop do
+        opt = NavOption.parse(data)
+        data = data[opt.size .. -1]
+        @options << opt
+        break if data.nil? || data.empty?
+      end
+    end
+  end
+end

data/lib/argus/nav_monitor.rb

@@ -0,0 +1,83 @@
+module Argus
+
+  class NavMonitor
+    attr_reader :streamer
+
+    def initialize(controller, remote_host)
+      @controller = controller
+      @streamer = NavStreamer.new(remote_host: remote_host)
+      @callbacks = []
+      @mutex = Mutex.new
+      @nav_data = nil
+      @nav_options = {}
+    end
+
+    def start
+      @streamer.start
+      @running = true
+      @nav_thread = Thread.new do
+        while @running
+          data = @streamer.receive_data
+          update_nav_data(data) if data
+        end
+      end
+    end
+
+    def stop
+      @running = false
+    end
+
+    def join
+      @nav_thread.join
+    end
+
+    def callback(callback=nil, &block)
+      @mutex.synchronize do
+        @callbacks << callback unless callback.nil?
+        @callbacks << block if block_given?
+      end
+    end
+
+    def data
+      @mutex.synchronize { @nav_data }
+    end
+
+    def option(tag)
+      @mutex.synchronize { @nav_options[tag] }
+    end
+
+    private
+
+    def update_nav_data(data)
+      @mutex.synchronize do
+        update_internal_nav_data(data)
+        do_callbacks(data)
+      end
+    rescue Exception => ex
+      puts "ERROR in callback: #{ex}"
+      puts ex.message
+      puts ex.backtrace
+      $stdout.flush
+    end
+
+    def update_internal_nav_data(data)
+      @nav_data = data
+      if @nav_data.bootstrap?
+        @controller.demo_mode
+      end
+      if @nav_data.control_command_ack?
+        @controller.ack_control_mode
+      end
+      data.options.each do |opt|
+        @nav_options[opt.tag] = opt if opt.tag < 100
+      end
+    end
+
+    def do_callbacks(data)
+      @callbacks.each do |callback|
+        callback.call(data)
+      end
+    end
+  end
+
+end

data/lib/argus/nav_option.rb

@@ -0,0 +1,35 @@
+module Argus
+
+  class NavOption
+    attr_reader :tag
+
+    def initialize(data)
+      @tag, @size = data.unpack("vv")
+    end
+
+    def size
+      @size < 4 ? 4 : @size
+    end
+
+    def self.options
+      @options ||= { }
+    end
+
+    def self.register(option)
+      options[option.tag] = option
+    end
+
+    def self.parse(raw_data)
+      tag = raw_data.unpack("v").first
+      option = options[tag] || NavOptionUnknown
+      option.new(raw_data)
+    end
+
+    # Skip the tag and size fields, We've already handled them in the
+    # base class.
+    def self.initial_format
+      "x4"
+    end
+  end
+
+end

data/lib/argus/nav_option_checksum.rb

@@ -0,0 +1,20 @@
+require 'argus/cfields'
+require 'argus/nav_option'
+require 'argus/nav_tag'
+
+module Argus
+
+  class NavOptionChecksum < NavOption
+    include CFields
+
+    uint32_t :chks
+    alias :checksum :chks
+
+    def self.tag
+      NavTag::CHECKSUM
+    end
+
+    NavOption.register(self)
+  end
+
+end

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 & 0x0f]
+    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