You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
44 lines
1.4 KiB
44 lines
1.4 KiB
#!/usr/bin/env python3 |
|
''' |
|
test script to check if all CAN GPS nodes are producing Fix2 frames at the expected rate |
|
''' |
|
|
|
import dronecan, time |
|
from dronecan import uavcan |
|
|
|
# get command line arguments |
|
from argparse import ArgumentParser |
|
parser = ArgumentParser(description='Fix2 gap example') |
|
parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate") |
|
parser.add_argument("--node-id", default=100, type=int, help="CAN node ID") |
|
parser.add_argument("--max-gap", default=0.25, type=float, help="max gap in seconds") |
|
parser.add_argument("port", default=None, type=str, help="serial port or mavcan URI") |
|
args = parser.parse_args() |
|
|
|
# Initializing a DroneCAN node instance. |
|
node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate) |
|
|
|
# Initializing a node monitor |
|
node_monitor = dronecan.app.node_monitor.NodeMonitor(node) |
|
|
|
last_fix2 = {} |
|
|
|
def handle_fix2(msg): |
|
nodeid = msg.transfer.source_node_id |
|
tstamp = msg.transfer.ts_real |
|
if not nodeid in last_fix2: |
|
last_fix2[nodeid] = tstamp |
|
return |
|
dt = tstamp - last_fix2[nodeid] |
|
last_fix2[nodeid] = tstamp |
|
if dt > args.max_gap: |
|
print("Node %u gap=%.3f" % (nodeid, dt)) |
|
|
|
# callback for printing ESC status message to stdout in human-readable YAML format. |
|
node.add_handler(dronecan.uavcan.equipment.gnss.Fix2, handle_fix2) |
|
|
|
while True: |
|
try: |
|
node.spin() |
|
except Exception as ex: |
|
print(ex)
|
|
|