Telegram Structure
The telegram in question is documented in Telegram Listing reference, starting on pages 97-115.
The telegram consists of space separated tokens, the numbers are generally hexadecimal. First, there is what you could call a header, of 18 tokens. I've briefly annotated the header from your example:
Command type sRA
Command LMDscandata
Version 0
Device number 1
Serial number 151FDC8
Device status 0 0
Telegram counter EC4B
Scan counter EDD5
Time since startup 85BF655E
Time of transmit 85BF9621
Digital inputs 0 0
Digital outputs 3E 0
Reserved/layer angle 0
Scanning frequency 2710 (100 Hz)
Measurement frequency 21C (54 kHz)
Next comes the variable part of the telegram, consisting of 8 sections. Each section starts with a count token, signifying how many blocks of data (if any) that section consists of. When the sensor is not configured to provide particular type of data, the count will be 0, and the next section immediately follows.
# of encoder blocks 0
[<encoder info>]
# of 16-bit channel blocks 1
[<channel blocks>]
# of 8-bit channel blocks 0
[<channel blocks>]
Position 0
[<position info>]
Device name 0
[<name text>]
Comment 0
[<comment text>]
Time 0
[<timestamp>]
Events 0
[<event info>]
In your case, the situation is simple, as there is only 1 block of 16-bit channel data. The layout of this block is:
Content DIST1 (Distance values of first pulse)
Scaling factor 3F800000 (1x)
Scale factor offset 00000000
Start angle 4A0E5 (30.3333 deg)
Angular step size 1A0B (0.6667 deg)
Value count B5 (181)
Data 305 ... DC6
Parsing the Telegram
With that out of the way, we can come up with a rudimentary parser for your specific sensor configuration:
- Split the telegram string into tokens, using space as the separator.
- Check that it is the expected command type and command
- Check that there are 0 encoder payload blocks
- Check that there is exactly 1 16-bit channel block
- Check that it is a 'DIST1' block
- Determine the scaling factor used (1x or 2x)
- Parse the start angle and angle step, and scale them to degrees
- Parse the value count
- Grab the appropriate number of value tokens that follow, discard the rest
** Parse each value, and scale it by the scaling factor
- Calculate the angles corresponding to each measured value (
start_angle + step * n)
A crude implementation of this in plain Python might look like this:
def parse_telegram(telegram):
tokens = telegram.split(' ')
assert(len(tokens) > (18 + 8)) # Minimum valid length
header = tokens[:18]
assert(header[0] == 'sRA') # Correct command type
assert(header[1] == 'LMDscandata') # Correct command
sections = tokens[18:]
assert(int(sections[0]) == 0) # No encoder data
assert(int(sections[1]) == 1) # Exactly 1 16-bit channel block
assert(sections[2] == 'DIST1') # Expected distance data
assert(sections[3] in ['3F800000', '40000000'])
scale_factor = 1 if sections[3] == '3F800000' else 2
assert(sections[4] == '00000000')
start_angle = int(sections[5], 16) / 10000.0
angle_step = int(sections[6], 16) / 10000.0
value_count = int(sections[7], 16)
values = list(map(lambda x: int(x, 16) * scale_factor, sections[8:8+value_count]))
# The following could be cached to avoid recalculation
# since it will be the same until sensor config is changed...
angles = [start_angle + angle_step * n for n in range(value_count)]
return (values, angles)
The function returns a tuple of two lists -- first containing the distances, second the corresponding beam angles. We can use matplotlib to plot this result on a polar plot to see if it makes sense:
Polar to Cartesian
Converting the polar coordinates to Cartesian is just a matter of applying some basic trigonometry:
x = r × cos(θ)
y = r × sin(θ)
In plain Python:
def to_cartesian(distances, angles):
x = list(map(lambda r, t: r * math.cos(math.radians(t)), distances, angles))
y = list(map(lambda r, t: r * math.sin(math.radians(t)), distances, angles))
return (x, y)
Again, a quick plot to check if the result makes sense:
