def _listen_socket(self):
"""
Listens to the socket and sleeps in between receives.
Runs forever (until disconnect is called)
"""
print("starting listening at ")
lasttime = time.time()
data = None
while (self.is_listening):
lasttime = time.time()
try:
(data, address) = self.udp_receive_sock.recvfrom(66000)
except socket.timeout:
print("timeout - trying again")
except:
pass
self.handle_data(data)
color_print("disconnecting", "INFO")
self.disconnect()
def handle_frame(self, packet_type, buffer_id, packet_seq_id, recv_data):
if (buffer_id == self.buffer_ids['PING']):
#color_print("this is a ping! need to pong", "INFO")
self._send_pong(recv_data)
if (self.data_types_by_number[packet_type] == 'ACK'):
#print("setting command received to true")
ack_seq = int(struct.unpack("<B", recv_data)[0])
self._set_command_received('SEND_WITH_ACK', True, ack_seq)
self.ack_packet(buffer_id, ack_seq)
elif (self.data_types_by_number[packet_type] == 'DATA_NO_ACK'):
#print("DATA NO ACK")
if (buffer_id in self.data_buffers):
self.drone.update_sensors(packet_type,
buffer_id,
packet_seq_id,
recv_data,
ack=False)
elif (self.data_types_by_number[packet_type] == 'LOW_LATENCY_DATA'):
print("Need to handle Low latency data")
elif (self.data_types_by_number[packet_type] == 'DATA_WITH_ACK'):
#print("DATA WITH ACK")
if (buffer_id in self.data_buffers):
self.drone.update_sensors(packet_type,
buffer_id,
packet_seq_id,
recv_data,
ack=True)
else:
color_print("HELP ME", "ERROR")
print("got a different type of data - help")
def update_sensors(self, data_type, buffer_id, sequence_number, raw_data,
ack):
"""
Update the sensors (called via the wifi or ble connection)
: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_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)
def __init__(self, address, use_wifi=False):
"""
Initialize with its BLE address - if you don't know the address, call findMambo
and that will discover it for you.
You can also connect to the wifi on the FPV camera. Do not use this if the camera is not connected. Also,
ensure you have connected your machine to the wifi on the camera before attempting this or it will not work.
:param address: unique address for this mambo
:param use_wifi: set to True to connect with wifi as well as the BLE
"""
self.address = address
self.use_wifi = use_wifi
if (use_wifi):
self.drone_connection = WifiConnection(self, drone_type="Mambo")
else:
if (BLEAvailable):
self.drone_connection = BLEConnection(address, self)
else:
self.drone_connection = None
color_print(
"ERROR: you are trying to use a BLE connection on a system that doesn't have BLE installed.",
"ERROR")
return
# intialize the command parser
self.command_parser = DroneCommandParser()
# initialize the sensors and the parser
self.sensors = MamboSensors()
self.sensor_parser = DroneSensorParser(drone_type="Mambo")
def send_command_packet_noack(self, packet):
"""
Sends the actual packet on the No-ack channel. Internal function only.
:param packet: packet constructed according to the command rules (variable size, constructed elsewhere)
:return: True if the command was sent and False otherwise
"""
try_num = 0
color_print("sending packet on try %d", try_num)
self.safe_send(packet)
def send_command_packet_ack(self, packet, seq_id):
"""
Sends the actual packet on the ack channel. Internal function only.
:param packet: packet constructed according to the command rules (variable size, constructed elsewhere)
:return: True if the command was sent and False otherwise
"""
try_num = 0
self._set_command_received('SEND_WITH_ACK', False, seq_id)
while (try_num < self.max_packet_retries
and not self._is_command_received('SEND_WITH_ACK', seq_id)):
color_print("sending packet on try %d", try_num)
self.safe_send(packet)
try_num += 1
self.smart_sleep(0.5)
return self._is_command_received('SEND_WITH_ACK', seq_id)
def safe_land(self, timeout):
"""
Ensure the mambo lands by sending the command until it shows landed on sensors
"""
start_time = time.time()
while (self.sensors.flying_state not in ("landing", "landed")
and (time.time() - start_time < timeout)):
if (self.sensors.flying_state == "emergency"):
return
color_print("trying to land", "INFO")
success = self.land()
self.smart_sleep(1)
while (self.sensors.flying_state != "landed"
and (time.time() - start_time < timeout)):
if (self.sensors.flying_state == "emergency"):
return
self.smart_sleep(1)
def connect(self, num_retries):
"""
Connects to the drone
:param num_retries: maximum number of retries
:return: True if the connection succeeded and False otherwise
"""
zeroconf = Zeroconf()
listener = mDNSListener(self)
browser = ServiceBrowser(zeroconf, self.mdns_address, listener)
# basically have to sleep until the info comes through on the listener
num_tries = 0
while (num_tries < num_retries and not self.is_connected):
time.sleep(1)
num_tries += 1
# if we didn't hear the listener, return False
if (not self.is_connected):
color_print(
"connection failed: did you remember to connect your machine to the Drone's wifi network?",
"ERROR")
return False
else:
browser.cancel()
# perform the handshake and get the UDP info
handshake = self._handshake(num_retries)
if (handshake):
self._create_udp_connection()
self.listener_thread = threading.Thread(target=self._listen_socket)
self.listener_thread.start()
color_print("Success in setting up the wifi network to the drone!",
"SUCCESS")
return True
else:
color_print("Error: TCP handshake failed.", "ERROR")
return False
def __init__(self, drone, drone_type="Bebop"):
"""
Can be a connection to a Bebop or a Mambo right now
:param type: type of drone to connect to
"""
self.is_connected = False
if (drone_type not in ("Bebop", "Mambo")):
color_print(
"Error: only type Bebop and Mambo are currently supported",
"ERROR")
return
self.drone = drone
self.drone_type = drone_type
self.udp_send_port = 0 # defined during the handshake
self.udp_receive_port = 43210
self.is_listening = True # for the UDP listener
if (drone_type == "Bebop"):
self.mdns_address = "_arsdk-090c._udp.local."
#Bebop video streaming
self.stream_port = 55004
self.stream_control_port = 55005
elif (drone_type == "Mambo"):
self.mdns_address = "_arsdk-090b._udp.local."
# map of the data types by name (for outgoing packets)
self.data_types_by_name = {
'ACK': 1,
'DATA_NO_ACK': 2,
'LOW_LATENCY_DATA': 3,
'DATA_WITH_ACK': 4
}
# map of the incoming data types by number (to figure out if we need to ack etc)
self.data_types_by_number = {
1: 'ACK',
2: 'DATA_NO_ACK',
3: 'LOW_LATENCY_DATA',
4: 'DATA_WITH_ACK'
}
self.sequence_counter = {
'PONG': 0,
'SEND_NO_ACK': 0,
'SEND_WITH_ACK': 0,
'SEND_HIGH_PRIORITY': 0,
'VIDEO_ACK': 0,
'ACK_DRONE_DATA': 0,
'NO_ACK_DRONE_DATA': 0,
'VIDEO_DATA': 0,
}
self.buffer_ids = {
'PING': 0, # pings from device
'PONG': 1, # respond to pings
'SEND_NO_ACK':
10, # not-ack commandsandsensors (piloting and camera rotations)
'SEND_WITH_ACK':
11, # ack commandsandsensors (all piloting commandsandsensors)
'SEND_HIGH_PRIORITY': 12, # emergency commandsandsensors
'VIDEO_ACK': 13, # ack for video
'ACK_DRONE_DATA': 127, # drone data that needs an ack
'NO_ACK_DRONE_DATA':
126, # data from drone (including battery and others), no ack
'VIDEO_DATA': 125, # video data
'ACK_FROM_SEND_WITH_ACK':
139 # 128 + buffer id for 'SEND_WITH_ACK' is 139
}
self.data_buffers = (self.buffer_ids['ACK_DRONE_DATA'],
self.buffer_ids['NO_ACK_DRONE_DATA'])
# store whether a command was acked
self.command_received = {
'SEND_WITH_ACK': False,
'SEND_HIGH_PRIORITY': False,
'ACK_COMMAND': False
}
# maximum number of times to try a packet before assuming it failed
self.max_packet_retries = 1
# threading lock for waiting
self._lock = threading.Lock()
def extract_sensor_values(self, data):
"""
Extract the sensor values from the data in the BLE packet
:param data: BLE packet of sensor data
:return: a list of tuples of (sensor name, sensor value, sensor enum, header_tuple)
"""
sensor_list = []
#print("updating sensors with ")
try:
header_tuple = struct.unpack_from("<BBH", data)
except:
color_print("Error: tried to parse a bad sensor packet", "ERROR")
return None
#print(header_tuple)
(names, data_sizes) = self._parse_sensor_tuple(header_tuple)
#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:
# figure out how to parse the data
(format_string, new_offset) = get_data_format_and_size(
data[packet_offset:], data_size)
if (new_offset == 0):
# 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
else:
# real data, parse it
sensor_data = struct.unpack_from(format_string,
data,
offset=packet_offset)
sensor_data = sensor_data[0]
if (data_size == "string"):
packet_offset += len(sensor_data)
else:
packet_offset += new_offset
except Exception as e:
sensor_data = None
#print(header_tuple)
color_print("Error parsing data for sensor", "ERROR")
print(e)
print("name of sensor is %s" % names)
print("data size is %s" % data_sizes)
print(len(data))
print(4 * (idx + 1))
#print("%s %s %s" % (name,idx,sensor_data))
#color_print("updating the sensor!", "NONE")
sensor_list.append(
[name, sensor_data, self.sensor_tuple_cache, header_tuple])
return sensor_list
else:
color_print(
"Could not find sensor in list - ignoring for now. Packet info below.",
"ERROR")
print(header_tuple)
#print(names)
return None
