Skip to content

Commit

Permalink
Fix reconnect behavior when rover code shuts down (#3)
Browse files Browse the repository at this point in the history 
* Fix reconnect behavior when rover code shuts down

* Fix wait forever behavior
  • Loading branch information
@abhaybd
abhaybd authored May 13, 2024
1 parent f70b82c commit 8d778f1
Showing 1 changed file with 54 additions and 53 deletions.
107 changes: 54 additions & 53 deletions ArduPilotDriver.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
setattr(collections, "MutableMapping", collections.abc.MutableMapping)
import dronekit


def debug_GPS_vars():
args = get_args()
print("Waiting for connection to {}...".format(args.port))
Expand All @@ -30,14 +31,20 @@ def debug_GPS_vars():
print("lat: {}, lon: {}, roll: {}, pitch: {}, yaw: {}"
.format(lat, lon, roll, pitch, yaw))


def get_args():
parser = argparse.ArgumentParser()
parser.add_argument("-p", "--port", default="/dev/ttyACM10", help="Serial port connected to ardupilot")
parser.add_argument("-hz", "--frequency", type=float, default=10.0, help="Rate at which data is sent")
parser.add_argument("-b", "--baud", default=57600, type=int, help="Baud rate of serial connection")
parser.add_argument("--debug", action="store_true", help="Prints debug info to stdout")
parser.add_argument("-p", "--port", default="/dev/ttyACM10",
help="Serial port connected to ardupilot")
parser.add_argument("-hz", "--frequency", type=float,
default=10.0, help="Rate at which data is sent")
parser.add_argument("-b", "--baud", default=57600,
type=int, help="Baud rate of serial connection")
parser.add_argument("--debug", action="store_true",
help="Prints debug info to stdout")
return parser.parse_args()


async def send_gps_message(websocket, lat, lon):
msg = {
"type": "gps",
Expand All @@ -46,6 +53,7 @@ async def send_gps_message(websocket, lat, lon):
}
await websocket.send(json.dumps(msg))


async def send_orientation_message(websocket, roll, pitch, yaw):
msg = {
"type": "orientation",
Expand All @@ -55,12 +63,6 @@ async def send_orientation_message(websocket, roll, pitch, yaw):
}
await websocket.send(json.dumps(msg))

async def send_heading_message(websocket, heading):
msg = {
"type": "heading",
"heading": heading
}
await websocket.send(json.dumps(msg))

async def async_main():
args = get_args()
Expand All @@ -86,54 +88,53 @@ def cleanup():
ardupilot.parameters["COMPASS_ORIENT"] = 6

print("Connecting to rover server...")
async for websocket in websockets.connect("ws://localhost:3001/ardupilot"):
print("Done!")
def gps_callback(self, _, frame):
curr_time = time.perf_counter()
if curr_time >= last_data_send_times["gps"] + 1./args.frequency:
if args.debug:
print("GPS Fix:", ardupilot.gps_0.fix_type, frame)
if ardupilot.gps_0.fix_type > 1:
last_data_send_times["gps"] = curr_time
asyncio.run(send_gps_message(websocket, frame.lat, frame.lon))

def orientation_callback(self, _, attitude):
curr_time = time.perf_counter()
if curr_time >= last_data_send_times["orientation"] + 1./args.frequency:
if args.debug:
print(attitude)
else:
pitch_deg = attitude.pitch * 180 / math.pi
roll_deg = attitude.roll * 180 / math.pi
print(f"\33[2K\rroll={roll_deg: 4.0f}, pitch={pitch_deg: 4.0f}", end="")
last_data_send_times["orientation"] = curr_time
asyncio.run(send_orientation_message(websocket, attitude.roll, attitude.pitch, attitude.yaw))

def heading_callback(self, _, heading):
curr_time = time.perf_counter()
if curr_time >= last_data_send_times["heading"] + 1./args.frequency:
if args.debug:
print("Heading", heading)
last_data_send_times["heading"] = curr_time
asyncio.run(send_heading_message(websocket, heading))

ardupilot.add_attribute_listener("location.global_frame", gps_callback)
ardupilot.add_attribute_listener("attitude", orientation_callback)
# ardupilot.add_attribute_listener("heading", heading_callback)

while True:
try:
while True:
await asyncio.sleep(1)
except websockets.ConnectionClosed as e:
print(e)
ardupilot.remove_attribute_listener("location.global_frame", gps_callback)
ardupilot.remove_attribute_listener("attitude", orientation_callback)
# ardupilot.remove_attribute_listener("heading", heading_callback)
continue
async with websockets.connect("ws://localhost:3001/ardupilot") as websocket:
print("Done!")

def gps_callback(self, _, frame):
curr_time = time.perf_counter()
if curr_time >= last_data_send_times["gps"] + 1./args.frequency:
if args.debug:
print("GPS Fix:", ardupilot.gps_0.fix_type, frame)
if ardupilot.gps_0.fix_type > 1:
last_data_send_times["gps"] = curr_time
asyncio.run(send_gps_message(websocket, frame.lat, frame.lon))

def orientation_callback(self, _, attitude):
curr_time = time.perf_counter()
if curr_time >= last_data_send_times["orientation"] + 1./args.frequency:
if args.debug:
print(attitude)
else:
pitch_deg = attitude.pitch * 180 / math.pi
roll_deg = attitude.roll * 180 / math.pi
print(f"\33[2K\rroll={roll_deg: 4.0f}, pitch={pitch_deg: 4.0f}", end="")
last_data_send_times["orientation"] = curr_time
asyncio.run(send_orientation_message(
websocket, attitude.roll, attitude.pitch, attitude.yaw))

ardupilot.add_attribute_listener("location.global_frame", gps_callback)
ardupilot.add_attribute_listener("attitude", orientation_callback)

try:
await asyncio.Future()
except websockets.ConnectionClosed as e:
print(e)
ardupilot.remove_attribute_listener("location.global_frame", gps_callback)
ardupilot.remove_attribute_listener("attitude", orientation_callback)
print("Reconnecting to rover server...")
continue
except (websockets.WebSocketException, OSError):
await asyncio.sleep(3)
pass


def main():
debug_GPS_vars()


if __name__ == "__main__":
asyncio.run(async_main())
# main()

0 comments on commit 8d778f1

Please sign in to comment.