hybridgroup-argus 0.2.0 → 0.3.0

checksums.yaml

@@ -0,0 +1,7 @@
+---
+SHA1:
+  metadata.gz: 61222b712f9c3d858153e12e2a386aa03e8e06a1
+  data.tar.gz: a0c4785df6cde5776eb45c997382b96f5de4e656
+SHA512:
+  metadata.gz: 7f306362aafe950603de3dba9d6efefbaa834a23b4fa27c6e655f85d146b702b115543665dc6d7ea1c273ace9cadbc5a4aec7f68fe3280a763ddbf2c3580d29d
+  data.tar.gz: 028aa4c216e1ebec8a6756635b54221ba87f6bfa6c6d34fb9d0f9ebd8226e464911048d2b9c954dbbb7d84cd46f19cbc138f163b76a9fdec3c480e20ea68e8c0

data/lib/argus/at_commander.rb

@@ -2,6 +2,8 @@ require 'thread'
 
 module Argus
   class ATCommander
+    attr_reader :timestamps
+
     def initialize(sender)
       @sender = sender
       @seq = 0
@@ -11,12 +13,14 @@ module Argus
       @thread = nil
       @interval = 0.020
       @mutex = Mutex.new
+      @timestamps = []
     end
 
     def start
       @running = true
       @thread = Thread.new do
         while @running
+          log_time
           tick
           sleep @interval
         end
@@ -53,11 +57,24 @@ module Argus
     end
 
     def config(key, value)
-      @buffer << "AT*CONFIG=605,\"#{key}\",\"#{value}\"\r"
+      @mutex.synchronize do
+        command("CONFIG", "\"#{key}\",\"#{value}\"")
+      end
+    end
+
+    def reset_watchdog
+      @mutex.synchronize do
+        command("COMWDG")
+      end
     end
 
     private
 
+    def log_time
+      @timestamps.shift if @timestamps.size >= 1000
+      @timestamps <<  Time.now.to_f
+    end
+
     def packet
       yield self
       flush
@@ -68,9 +85,13 @@ module Argus
       @buffer = ""
     end
 
-    def command(name, args)
+    def command(name, args=nil)
       @seq += 1
-      @buffer << "AT*#{name}=#{@seq},#{args}\r"
+      if args
+        @buffer << "AT*#{name}=#{@seq},#{args}\r"
+      else
+        @buffer << "AT*#{name}=#{@seq}\r"
+      end
     end
   end
 end

data/lib/argus/cad_type.rb

@@ -0,0 +1,22 @@
+module Argus
+  module CadType
+
+    NAMES = [
+      :horizontal,              # Deprecated
+      :vertical,                # Deprecated
+      :vision,                  # Detection of 2D horizontal tags on drone shells
+      :none,                    # Detection disabled
+      :cocarde,                 # Detects a roundel under the drone
+      :oriented_cocarde,        # Detects an oriented roundel under the drone
+      :stripe,                  # Detects a uniform stripe on the ground
+      :h_cocarde,               # Detects a roundel in front of the drone
+      :h_oriented_cocarde,      # Detects an oriented roundel in front of the drone
+      :stripe_v,
+      :multiple_detection_mode, # The drone uses several detections at the same time
+      :cap,                     # Detects a Cap orange and green in front of the drone
+      :oriented_cocarde_bw,     # Detects the black and white roundel
+      :vision_v2,               # Detects 2nd version of shell/tag in front of the drone
+      :tower_side,              # Detect a tower side with the front camera
+    ]
+  end
+end

data/lib/argus/cfields.rb

@@ -0,0 +1,98 @@
+require 'argus/float_encoding'
+
+module Argus
+
+  module CFields
+    include FloatEncoding
+
+    def self.included(base)
+      base.send :extend, ClassMethods
+    end
+
+    def initialize(*args)
+      super
+      @data = unpack_data(args.first)
+    end
+
+    def unpack_data(data)
+      @data = data.unpack(self.class.format_string)
+    end
+
+    module ClassMethods
+      def data_index
+        @data_index ||= 0
+      end
+
+      def format_string
+        @format_string ||= "x4"
+      end
+
+      def allot(n=1)
+        result = data_index
+        @data_index += n
+        result
+      end
+
+      def define_field(name, size, format, width=1, &transform)
+        if size
+          define_array_field(name, size, format, width, transform)
+        else
+          define_scaler_field(name, format, width, transform)
+        end
+      end
+
+      def define_scaler_field(name, format, width, transform)
+        index = allot(width)
+        format_string << (width==1 ? format : "#{format}#{width}")
+        if transform
+          ivar = "@#{name}".to_sym
+          define_method(name) {
+            instance_variable_get(ivar) ||
+              instance_variable_set(ivar, transform.call(@data[index]))
+          }
+        else
+          define_method(name) { @data[index] }
+        end
+      end
+
+      def define_array_field(name, size, format, width, transform)
+        index = allot(width*size)
+        format_string << "#{format}#{width*size}"
+        if transform
+          ivar = "@#{name}".to_sym
+          define_method(name) {
+            instance_variable_get(ivar) ||
+              instance_variable_set(ivar, @data[index, size].map(&transform))
+          }
+        else
+          define_method(name) { @data[index, size] }
+        end
+      end
+
+      def uint32_t(name, size=nil)
+        define_field(name, size, "V")
+      end
+
+      def uint16_t(name, size=nil)
+        define_field(name, size, "v")
+      end
+
+      def float32_t(name, size=nil)
+        define_field(name, size, "V") { |v| FloatEncoding.decode_float(v) }
+      end
+
+      def int32_t(name, size=nil)
+        define_field(name, size, "l<")
+      end
+
+      def matrix33_t(name, size=nil)
+        define_field(name, size, "V", 9) { |v| nil }
+      end
+
+      def vector31_t(name, size=nil)
+        define_field(name, size, "V", 3) { |v| nil }
+      end
+    end
+  end
+
+end

data/lib/argus/controller.rb

@@ -1,5 +1,8 @@
+require 'argus/float_encoding'
+
 module Argus
   class Controller
+    include FloatEncoding
 
     def initialize(at_commander)
       @at_commander = at_commander
@@ -83,14 +86,44 @@ module Argus
       update_pcmd
     end
 
+    def led(selection, hertz, duration)
+      selection = LedAnimation.lookup_value(selection)
+      value = [
+        selection,
+        FloatEncoding.encode_float(hertz),
+        duration
+      ].join(',')
+      @at_commander.config("leds:leds_anim",value)
+      self
+    end
+
+    def enable_detection(colors, type=10, select=32)
+      config("detect:enemy_colors",colors.to_s)
+      config("detect:detect_type", type.to_s)
+      config("detect:detections_select_h", select.to_s)
+      self
+    end
+
+    def config(key, value)
+      @at_commander.config(key, value)
+    end
+
+    def demo_mode
+      @at_commander.config("general:navdata_demo", "TRUE")
+    end
+
     def front_camera
       @at_commander.config("video:video_channel", "2")
     end
-    
+
     def bottom_camera
       @at_commander.config("video:video_channel", "1")
     end
 
+    def reset_watchdog
+      @at_commander.reset_watchdog
+    end
+
     private
 
     REF_BASE = [18, 20, 22, 24, 28].
@@ -122,9 +155,5 @@ module Argus
       self
     end
 
-    def encode_float(float)
-      [float].pack('g').unpack("l>").first
-    end
-
   end
 end

data/lib/argus/drone.rb

@@ -1,22 +1,47 @@
 require 'socket'
+require 'argus/nav_monitor'
 
 module Argus
+
   class Drone
+    attr_reader :controller, :enable_nav_monitor
+
     def initialize(socket=nil, host='192.168.1.1', port='5556')
       @socket = socket || UDPSocket.new
       @sender = Argus::UdpSender.new(@socket, host, port)
       @at = Argus::ATCommander.new(@sender)
       @controller = Argus::Controller.new(@at)
+      
+      @enable_nav_monitor = false
     end
 
-    def start
+    def commander
+      @at
+    end
+
+    def start(enable_nav_monitor=true)
+      @enable_nav_monitor = enable_nav_monitor
+      
+      if enable_nav_monitor
+        @nav = NavMonitor.new(@controller)
+        @nav.start
+      end
+
       @at.start
     end
 
     def stop
       @controller.land
+
       @at.stop
+      @nav.stop if enable_nav_monitor
+
       @at.join
+      @nav.join if enable_nav_monitor
+    end
+
+    def nav_callback(*args, &block)
+      @nav.callback(*args, &block)
     end
 
     %w(
@@ -26,6 +51,8 @@ module Argus
        up down
        turn_left turn_right
        front_camera bottom_camera
+       config
+       reset_watchdog
     ).each do |meth|
       define_method(meth) { |*args|
         @controller.send(meth, *args)

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

@@ -1,61 +1,78 @@
 module Argus
   class NavData
-    attr_accessor :drone_state, :options, :vision_flag, :sequence_number
+    attr_accessor :state_mask, :vision_flag, :sequence_number, :options
+
     def initialize(data)
       @data = data
       parse_nav_data(@data)
     end
 
-    private 
+    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)
-      state = data.unpack("V").first
-      @drone_state = {
-        :flying=>state[0],  #/*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */
-        :videoEnabled=>state[1],  #/*!< VIDEO MASK : (0) video disable, (1) video enable */
-        :visionEnabled=>state[2],  #/*!< VISION MASK : (0) vision disable, (1) vision enable */
-        :controlAlgorithm=>state[3],  #/*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */
-        :altitudeControlAlgorithm=>state[4],  #/*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
-        :startButtonState=>state[5],  #/*!< USER feedback : Start button state */
-        :controlCommandAck=>state[6],  #/*!< Control command ACK : (0) None, (1) one received */
-        :cameraReady=>state[7],  #/*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */
-        :travellingEnabled=>state[8],  #/*!< Travelling mask : (0) disable, (1) enable */
-        :usbReady=>state[9],  #/*!< USB key : (0) usb key not ready, (1) usb key ready */
-        :navdataDemo=>state[10], #/*!< Navdata demo : (0) All navdata, (1) only navdata demo */
-        :navdataBootstrap=>state[11], #/*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
-        :motorProblem=>state[12], #/*!< Motors status : (0) Ok, (1) Motors problem */
-        :communicationLost=>state[13], #/*!< Communication Lost : (1) com problem, (0) Com is ok */
-        :softwareFault=>state[14], #/*!< Software fault detected - user should land as quick as possible (1) */
-        :lowBattery=>state[15], #/*!< VBat low : (1) too low, (0) Ok */
-        :userEmergencyLanding=>state[16], #/*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
-        :timerElapsed=>state[17], #/*!< Timer elapsed : (1) elapsed, (0) not elapsed */
-        :MagnometerNeedsCalibration=>state[18], #/*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */
-        :anglesOutOfRange=>state[19], #/*!< Angles : (0) Ok, (1) out of range */
-        :tooMuchWind=>state[20], #/*!< WIND MASK: (0) ok, (1) Too much wind */
-        :ultrasonicSensorDeaf=>state[21], #/*!< Ultrasonic sensor : (0) Ok, (1) deaf */
-        :cutoutDetected=>state[22], #/*!< Cutout system detection : (0) Not detected, (1) detected */
-        :picVersionNumberOk=>state[23], #/*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
-        :atCodecThreadOn=>state[24], #/*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
-        :navdataThreadOn=>state[25], #/*!< Navdata thread ON : (0) thread OFF (1) thread ON */
-        :videoThreadOn=>state[26], #/*!< Video thread ON : (0) thread OFF (1) thread ON */
-        :acquisitionThreadOn=>state[27], #/*!< Acquisition thread ON : (0) thread OFF (1) thread ON */
-        :controlWatchdogDelay=>state[28], #/*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
-        :adcWatchdogDelay=>state[29], #/*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
-        :comWatchdogProblem=>state[30], #/*!< Communication Watchdog : (1) com problem, (0) Com is ok */
-        :emergencyLanding=>state[31]  #/*!< Emergency landing : (0) no emergency, (1) emergency */
-      }
-      data.slice!(0..3)
-      @sequence_number = data.unpack("V").first
-      data.slice!(0..3)
-      @vision_flag = data.unpack("V").first
+      tag, @state_mask, @sequence_number, @vision_flag = data.unpack("VVVV")
+      parse_nav_options(data[16..-1])
+    end
+
+    def parse_nav_options(data)
       @options = []
-      option1 = {}
-      data.slice!(0..3)
-      option1[:id] = data.unpack("v").first
-      data.slice!(0..1)
-      option1[:size] = data.unpack("v").first
-      data.slice!(0..1)
-      option1[:data] = data.unpack("V").first
-      @options.push(option1)
+      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,78 @@
+module Argus
+
+  class NavMonitor
+    def initialize(controller)
+      @controller = controller
+      @nav = NavStreamer.new
+      @callbacks = []
+      @mutex = Mutex.new
+      @nav_data = nil
+      @nav_options = {}
+    end
+
+    def start
+      @nav.start
+      @running = true
+      @nav_thread = Thread.new do
+        while @running
+          data = @nav.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
+      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,29 @@
+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
+  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