Skip to content

Commit

Permalink
Apply suggestions from code review
Browse files Browse the repository at this point in the history
Co-authored-by: ES-Alexander <[email protected]>
  • Loading branch information
Williangalvani and ES-Alexander authored Aug 9, 2022
1 parent 5643401 commit 8b171d8
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 30 deletions.
11 changes: 5 additions & 6 deletions core/services/ping/ping1d_driver.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,15 +17,14 @@ def __init__(self, ping: PingDeviceDescriptor, port: int) -> None:
self.manager = Manager(SERVICE_NAME, SettingsV1)
# our settings file is a list for each sensor type.
# check the list to find our current sensor in it
settings = list(filter(lambda x: x.port == self.ping.get_hw_or_eth_info(), self.manager.settings.ping1d_specs))
connection_info = self.ping.get_hw_or_eth_info()
settings = [ping1d for ping1d in self.manager.settings.ping1d_specs if ping1d.port == connection_info]
# if it is not there, we create a new entry
if not settings:
self.manager.settings.ping1d_specs.append(Ping1dSettingsSpecV1.new(self.ping.get_hw_or_eth_info(), False))
self.manager.settings.ping1d_specs.append(Ping1dSettingsSpecV1.new(connection_info, False))
self.manager.save()
# read settings again
our_settings = list(
filter(lambda x: x.port == self.ping.get_hw_or_eth_info(), self.manager.settings.ping1d_specs)
)[0]
# read settings again, and extract first (and only) result
our_settings, = [ping1d for ping1d in self.manager.settings.ping1d_specs if ping1d.port == connection_info]
self.driver_status["mavlink_driver_enabled"] = our_settings.mavlink_enabled
self.mavlink_driver = Ping1DMavlinkDriver(our_settings.mavlink_enabled)

Expand Down
30 changes: 15 additions & 15 deletions core/services/ping/ping1d_mavlink.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
from loguru import logger

## The minimum interval time for distance updates to the autopilot
PING_INTERVAL_MS = 0.2
PING_INTERVAL_S = 0.2


class Ping1DMavlinkDriver:
Expand All @@ -48,7 +48,7 @@ def distance_message(time_boot_ms: int, distance: int, device_id: int) -> Dict[s
"min_distance": 20,
"max_distance": 5000,
"current_distance": int(distance),
"mavtype": {"type": "MAV_DISTANCE_SENSOR_LASER"},
"mavtype": {"type": "MAV_DISTANCE_SENSOR_ULTRASOUND"},
"id": device_id,
"orientation": {"type": "MAV_SENSOR_ROTATION_PITCH_270"},
"covariance": 0,
Expand All @@ -63,7 +63,7 @@ async def send_distance_data(self, distance: int, deviceid: int, confidence: int
# logger.debug("sending distance %d confidence %d" % (distance, confidence))
if confidence < 0.5:
distance = 0
logger.debug(f"sendind {distance}")
logger.debug(f"sending {distance=}")
await self.mavlink2rest.send_mavlink_message(
self.distance_message(int((time.time() - self.tboot) * 1000), int(distance / 10), deviceid)
)
Expand Down Expand Up @@ -102,7 +102,7 @@ async def drive(self, port: int) -> None:
data = PingMessage()
data.request_id = PING1D_SET_PING_INTERVAL
data.src_device_id = 0
data.ping_interval = int(PING_INTERVAL_MS * 1000)
data.ping_interval = int(PING_INTERVAL_S * 1000)
data.pack_msg_data()
self.ping1d_io.sendall(data.msg_data)

Expand All @@ -111,19 +111,19 @@ async def drive(self, port: int) -> None:
await asyncio.sleep(1)
continue
await asyncio.sleep(0.001)
tnow = time.time()
tnow = time.perf_counter()

# request data from ping device
if tnow > last_distance_measurement_time + PING_INTERVAL_MS:
if tnow > last_ping_request_time + PING_INTERVAL_MS:
last_ping_request_time = time.time()
if tnow > last_distance_measurement_time + PING_INTERVAL_S:
if tnow > last_ping_request_time + PING_INTERVAL_S:
last_ping_request_time = time.perf_counter()
await self.send_ping1d_request()

# deal with possibly lost connection
if tnow > last_distance_measurement_time + PING_INTERVAL_MS * 10:
logger.info("attempting reconnection...")
self.ping1d_io.connect(pingserver)
await asyncio.sleep(0.1)
# deal with possibly lost connection
if tnow > last_distance_measurement_time + PING_INTERVAL_S * 10:
logger.info("attempting reconnection...")
self.ping1d_io.connect(pingserver)
await asyncio.sleep(0.1)

# read data in from ping device
try:
Expand All @@ -137,15 +137,15 @@ async def drive(self, port: int) -> None:
return
continue

if tnow - last_distance_measurement_time < PING_INTERVAL_MS * 0.8:
if tnow - last_distance_measurement_time < PING_INTERVAL_S * 0.8:
# skip decoding data to save cpu if it is arriving too fast
continue
# decode data from ping device, forward to autopilot
for byte in data:
try:
if ping_parser.parse_byte(byte) == PingParser.NEW_MESSAGE:
if ping_parser.rx_msg.message_id in distance_messages:
last_distance_measurement_time = time.time()
last_distance_measurement_time = time.perf_counter()
distance = ping_parser.rx_msg.distance
deviceid = ping_parser.rx_msg.src_device_id
confidence = ping_parser.rx_msg.confidence
Expand Down
6 changes: 3 additions & 3 deletions core/services/ping/ping360_ethernet_prober.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@ def list_ips() -> List[str]:
if interface in stats and getattr(stats[interface], "isup"):
available_networks.append(interface)

network_interface_addresses = psutil.net_if_addrs()
for interface in available_networks:
phys_list = list(filter(lambda address: address.family == socket.AF_INET, psutil.net_if_addrs()[interface]))
ips.extend([phys.address for phys in phys_list])
ips.extend(phys.address for phys in network_interface_addresses[interface] if phys.family == socket.AF_INET)
return list(set(ips)) # make sure there are not duplicated entries


Expand All @@ -33,7 +33,7 @@ def remove_zeros(ip: str) -> str:
This removes the leading zeros return by the Ping360 firmware.
This functions transforms 192.168.015.016 into 192.168.15.16
"""
new_ip = ".".join([str(int(i)) for i in ip.split(".")])
new_ip = ".".join(str(int(i)) for i in ip.split("."))
return new_ip


Expand Down
4 changes: 2 additions & 2 deletions core/services/ping/pingmanager.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ def __init__(self) -> None:

def stop_driver_at_port(self, port: Serial) -> None:
"""Stops the driver instance running for port "port" """
ping_at_port = list(filter(lambda ping: ping.port == port, self.drivers.keys()))
ping_at_port = [ping for ping in self.drivers if ping.port == port]
if ping_at_port:
self.drivers[ping_at_port[0]].stop()
del self.drivers[ping_at_port[0]]
Expand All @@ -34,7 +34,7 @@ def find_next_port(self, base_port: int, direction: int) -> int:
"""
port = base_port
while udp_port_is_in_use(port):
port = port + direction
port += direction
return port

async def launch_driver_instance(self, ping: PingDeviceDescriptor) -> None:
Expand Down
6 changes: 2 additions & 4 deletions core/services/ping/pingutils.py
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,5 @@ def __str__(self) -> str:


def udp_port_is_in_use(port: int) -> bool:
for conn in psutil.net_connections():
if conn.laddr.port == port and conn.type == socket.SocketKind.SOCK_DGRAM:
return True
return False
return any(conn.laddr.port == port and conn.type == socket.SocketKind.SOCK_DGRAM
for conn in psutil.net_connections())

0 comments on commit 8b171d8

Please sign in to comment.