Class: Crubyflie::TOC

Inherits:
Object
  • Object
show all
Includes:
CRTPConstants, Logging
Defined in:
lib/crubyflie/crazyflie/toc.rb

Overview

A Table Of Contents It is a hash that stores a group index. Each group is a Hash indexed by element ID that stores TOC element

Constant Summary

Constants included from CRTPConstants

CRTPConstants::CMD_APPEND_BLOCK, CRTPConstants::CMD_CREATE_BLOCK, CRTPConstants::CMD_DELETE_BLOCK, CRTPConstants::CMD_RESET_LOGGING, CRTPConstants::CMD_START_LOGGING, CRTPConstants::CMD_STOP_LOGGING, CRTPConstants::CMD_TOC_ELEMENT, CRTPConstants::CMD_TOC_INFO, CRTPConstants::CRTP_PORTS, CRTPConstants::LOG_DATA_CHANNEL, CRTPConstants::LOG_SETTINGS_CHANNEL, CRTPConstants::PARAM_READ_CHANNEL, CRTPConstants::PARAM_WRITE_CHANNEL, CRTPConstants::TOC_CHANNEL, CRTPConstants::WAIT_PACKET_TIMEOUT

Instance Attribute Summary collapse

Instance Method Summary collapse

Methods included from Logging

logger, #logger, #logger=

Constructor Details

#initialize(cache_folder = nil, element_class = TOCElement) ⇒ TOC

Initializes the hash

Parameters:

  • cache_folder (String) (defaults to: nil) —

    where is the cache for this toc stored

  • element_class (Class) (defaults to: TOCElement) —

    the class type we should instantiate TOC elements to, when fetching them from the crazyflie



53
54
55
56
57
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 53

def initialize(cache_folder=nil, element_class=TOCElement)
    @toc = {}
    @cache = TOCCache.new(cache_folder)
    @element_class = element_class
end

Instance Attribute Details

#toc ⇒ Object (readonly)

Returns the value of attribute toc.



47
48
49
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 47

def toc
  @toc
end

Instance Method Details

#[](name_or_id, mode = :both) ⇒ TocElement?

Get a TOC element

Parameters:

  • name_or_id (String, Symbol) —

    name or ident of the element

  • mode (Symbol) (defaults to: :both) —

    get it :by_name, :by_id, :both

Returns:

  • (TocElement, nil) —

    the element or nil if not found



63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 63

def [](name_or_id, mode=:both)
    by_name_element = nil
    by_id_element   = nil
    # Find by name
    if [:both, :by_name].include?(mode)
        group = name = nil
        if name_or_id.is_a?(String)
            group, name = name_or_id.split(".", 2)
        end

        if name.nil?
            name = group
            group = nil
        end

        if group
            gr = @toc[group]
            by_name_element = gr[name] if gr
        else
            @toc.each do |group_name, group|
                candidate = group[name]
                by_name_element = candidate if candidate
                break if candidate
            end
        end
    end

    if [:both, :by_id].include?(mode)
        @toc.each do |group_name, group|
            group.each do |name, element|
                by_id_element = element if element.ident == name_or_id
                break if by_id_element
            end
        end
    end

    return by_name_element || by_id_element
end

#export_to_cache(crc) ⇒ Object

Saves this TOC into cache



113
114
115
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 113

def export_to_cache(crc)
    @cache.insert(crc, @toc)
end

#fetch_from_crazyflie(crazyflie, port, in_queue) ⇒ Object

Fetches a TOC from crazyflie in a synchronous way Instead of a TOCFetcher (as in the python library), we just found easier to take advantage of the facility queues and read them (and block when there is nothing to read) to initialize the TOC. Doing it this way not only saves to have to register and chain callbacks unrelated places (as Crazyflie class), but also to reorder incoming TOCElement packages if they come unordered (we just requeue them). This might happen if a package needs to be resent and the answer for the next one comes earlier.

This function should be preferably called before starting any other activities (send/receive threads) in the relevant facilities.

Parameters:

  • crazyflie (Crazyflie) —

    used to send packages

  • port (Integer) —

    the port to send the packages

  • in_queue (Integer) —

    a queue on which the responses to the sent packages are queued



151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 151

def fetch_from_crazyflie(crazyflie, port, in_queue)
    # http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol
    # #table_of_content_access
    packet = CRTPPacket.new(0, [CMD_TOC_INFO])
    packet.modify_header(nil, port, TOC_CHANNEL)
    in_queue.clear()

    crazyflie.send_packet(packet, true)
    response = in_queue.pop() # we block here if none :)
    while response.channel != TOC_CHANNEL do
        in_queue << response
        logger.debug "Got a non-TOC packet. Requeueing..."
        Thread.pass
        # @todo timeout
        response = in_queue.pop()
    end
    data = response.data
    # command = data[0]
    # Repack the payload
    payload = response.data_repack[1..-1] # get rid of byte 0
    # The crc comes in an unsigned int (L) in little endian (<)
    total_toc_items, crc = payload[0..5].unpack('CL<')
    hex_crc = crc.to_s(16)

    logger.debug "TOC crc #{hex_crc}, #{total_toc_items} items"
    import_from_cache(hex_crc)

    if !@toc.nil? # in cache so we can stop here
        logger.debug "TOC found in cache"
        return
    end

    logger.debug "Not in cache"
    @toc = {}
    # We proceed to request all the TOC elements
    requested_item = 0
    while requested_item < total_toc_items do
        wait = WAIT_PACKET_TIMEOUT
        request_toc_element(crazyflie, requested_item, port)

        begin
            response = timeout(wait, WaitTimeoutException) do
                response = in_queue.pop() # block here
            end
        rescue
            retries ||= 0
            retries += 1
            retry if retries < 2
            logger.error("Timeout reached waiting for TOC element")
            raise $!
        end
        if response.channel != TOC_CHANNEL
            # Requeue
            in_queue << response
            mesg =  "Got a non-TOC packet on #{response.channel}."
            mesg << " Requeueing..."
            logger.debug(mesg)
            Thread.pass
            next
        end
        payload = response.data_repack()[1..-1] # leave byte 0 out
        toc_elem = @element_class.new(payload)
        if (a = requested_item) != (b = toc_elem.ident())
            logger.debug "#{port}: Expected #{a}, but got #{b}"
            if b > a
                logger.debug "Requeing"
                # this way we are ordering items
                in_queue << response
            end
            next
        end

        insert(toc_elem)
        logger.debug("Added #{toc_elem.ident} to TOC")
        requested_item += 1
    end
    export_to_cache(hex_crc)
end

#import_from_cache(crc) ⇒ Object

Retrieves this TOC from the cache



118
119
120
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 118

def import_from_cache(crc)
    @toc = @cache.fetch(crc)
end

#insert(element) ⇒ Object Also known as: <<

Insert a TOC element in the TOC

Parameters:

  • element (TocElement) —

    the name of the element group



104
105
106
107
108
109
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 104

def insert(element)
    group = element.group
    name = element.name
    @toc[group] = {} if @toc[group].nil?
    @toc[group][name] = element
end

#to_s ⇒ String

Produce a simple string representation of the TOC

Returns:

  • (String) —

    a pretty string



124
125
126
127
128
129
130
131
132
133
 
# File 'lib/crubyflie/crazyflie/toc.rb', line 124

def to_s
    s = ""
    @toc.each do |group,v|
        s << "- #{group}\n"
        v.each do |name, elem|
            s << "    * #{name} (#{elem.ctype}/##{elem.type_id})\n"
        end
    end
    return s
end