Skip to content

Commit

Permalink
Fixed drone parser to handle lists of values correctly and also fixed…
Browse files Browse the repository at this point in the history 
… the Mambo sensors to handle the lists as well as fixed two typos in sensors
  • Loading branch information
@amymcgovern
amymcgovern committed Dec 3, 2017
1 parent daf8f3c commit f4b4d5b
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 24 deletions.
32 changes: 21 additions & 11 deletions Mambo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,14 @@ def __init__(self):
self.speed_ts = 0

# these are only available on wifi
self.altitude = None
self.altitude = -1
self.altitude_ts = 0

self.quaternion_w = None
self.quaternion_x = None
self.quaternion_y = None
self.quaternion_z = None
self.quaternion_ts = None
self.quaternion_w = -1
self.quaternion_x = -1
self.quaternion_y = -1
self.quaternion_z = -1
self.quaternion_ts = -1

def update(self, name, value, sensor_enum):
"""
Expand Down Expand Up @@ -101,7 +101,7 @@ def update(self, name, value, sensor_enum):
self.speed_ts = value
elif (name == "DroneAltitude_altitude"):
self.altitude = value
elif (name == "DroneAltitude_altitude_ts"):
elif (name == "DroneAltitude_ts"):
self.altitude_ts = value
elif (name == "DroneQuaternion_q_w"):
self.quaternion_w = value
Expand All @@ -111,7 +111,7 @@ def update(self, name, value, sensor_enum):
self.quaternion_y = value
elif (name == "DroneQuaternion_q_z"):
self.quaternion_z = value
elif (name == "DroneQuaternion_tz"):
elif (name == "DroneQuaternion_ts"):
self.quaternion_ts = value
else:
#print "new sensor - add me to the struct but saving in the dict for now"
Expand Down Expand Up @@ -177,9 +177,19 @@ def update_sensors(self, data_type, buffer_id, sequence_number, raw_data, ack):
:param data: raw data packet that needs to be parsed
:param ack: True if this packet needs to be ack'd and False otherwise
"""
(sensor_name, sensor_value, sensor_enum, header_tuple) = self.sensor_parser.extract_sensor_values(raw_data)
self.sensors.update(sensor_name, sensor_value, sensor_enum)
#print(self.sensors)

sensor_list = self.sensor_parser.extract_sensor_values(raw_data)
if (sensor_list is not None):
for sensor in sensor_list:
(sensor_name, sensor_value, sensor_enum, header_tuple) = sensor
if (sensor_name is not None):
self.sensors.update(sensor_name, sensor_value, sensor_enum)
# print(self.sensors)
else:
color_print(
"data type %d buffer id %d sequence number %d" % (data_type, buffer_id, sequence_number),
"WARN")
color_print("This sensor is missing (likely because we don't need it)", "WARN")

if (ack):
self.drone_connection.ack_packet(buffer_id, sequence_number)
Expand Down
47 changes: 34 additions & 13 deletions commandsandsensors/DroneSensorParser.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,51 +44,72 @@ def extract_sensor_values(self, data):
#print("name of sensor is %s" % names)
#print("data size is %s" % data_sizes)

packet_offset = 4
if names is not None:
for idx, name in enumerate(names):
data_size = data_sizes[idx]
try:
if (data_size == "u8" or data_size == "enum"):
# unsigned 8 bit, single byte
sensor_data = struct.unpack_from("<B", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<B", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 1
elif (data_size == "i8"):
# signed 8 bit, single byte
sensor_data = struct.unpack_from("<b", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<b", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 1
elif (data_size == "u16"):
sensor_data = struct.unpack_from("<H", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<H", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 2
elif (data_size == "i16"):
sensor_data = struct.unpack_from("<h", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<h", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 2
elif (data_size == "u32"):
sensor_data = struct.unpack_from("<I", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<I", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 4
elif (data_size == "i32"):
sensor_data = struct.unpack_from("<i", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<i", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 4
elif (data_size == "u64"):
sensor_data = struct.unpack_from("<Q", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<Q", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 8
elif (data_size == "i64"):
sensor_data = struct.unpack_from("<q", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<q", data, offset=packet_offset)
sensor_data = int(sensor_data[0])
packet_offset += 8
elif (data_size == "float"):
sensor_data = struct.unpack_from("<f", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<f", data, offset=packet_offset)
sensor_data = float(sensor_data[0])
packet_offset += 4
elif (data_size == "double"):
sensor_data = struct.unpack_from("<d", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<d", data, offset=packet_offset)
sensor_data = float(sensor_data[0])
packet_offset += 8
elif (data_size == "string"):
# string
sensor_data = struct.unpack_from("<s", data, offset=4*(idx+1))
sensor_data = struct.unpack_from("<s", data, offset=packet_offset)
sensor_data = sensor_data[0]
packet_offset += len(sensor_data)
else:
sensor_data = None
color_print("Write the parser for this value", "ERROR")
#color_print("Write the parser for this value", "ERROR")
#print("name of sensor is %s" % names)
#print("data size is %s" % data_sizes)
# this is usually a boolean flag stating that values have changed so set the value to True
# and let it return the name
sensor_data = True
except:
sensor_data = None
#print(header_tuple)
print("name of sensor is %s" % names)
print("data size is %s" % data_sizes)
print(len(data))
print(4*(idx+1))
color_print("Error parsing data for sensor", "ERROR")

#print("%s %s %s" % (name,idx,sensor_data))
Expand Down

0 comments on commit f4b4d5b

Please sign in to comment.