Created
November 2, 2020 23:43
-
-
Save jaxxzer/f10a9c316339fd1c419a7f818b5df8bd to your computer and use it in GitHub Desktop.
Erase parameters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/python | |
# - erase autopilot parameters (reset to defaults) | |
# - reboot autopilot to apply changes | |
import os | |
import time | |
from pymavlink import mavutil | |
from argparse import ArgumentParser | |
timeout = 1 | |
parser = ArgumentParser(description=__doc__) | |
parser.add_argument("--port", dest="port", required=True, help="Serial port of the device to erase") | |
parser.add_argument("--baudrate", dest="baudrate", type=int, default=115200, help="Baudrate to use for device communications") | |
args = parser.parse_args() | |
# time.sleep(6) | |
while not os.path.exists(args.port): | |
print("waiting for", args.port) | |
time.sleep(0.5) | |
#raise Exception("port does not exist: %s" % args.port) | |
master = mavutil.mavlink_connection(args.port, args.baudrate) | |
master.wait_heartbeat() | |
# Reset parameters to firmware defaults | |
print("Resetting parameters to defaults.",) | |
master.mav.command_long_send( | |
1, # target system | |
1, # target component | |
mavutil.mavlink.MAV_CMD_PREFLIGHT_STORAGE, # command | |
0, # confirmation | |
2, # erase | |
0,0,0,0,0,0) # unused params | |
verified = False | |
start = time.time() | |
while time.time() < start + timeout: | |
msg = master.recv_match() | |
if msg is not None: | |
if msg.get_type() == "COMMAND_ACK" and msg.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_STORAGE and msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: | |
print(" OK") | |
verified = True | |
break | |
time.sleep(0.01) | |
if not verified: | |
print(" FAIL!") | |
exit(1) | |
print("Rebooting.",) | |
master.mav.command_long_send( | |
1, # target system | |
1, # target component | |
mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # command | |
0, # confirmation | |
1, # reboot | |
0,0,0,0,0,0) # unused params | |
verified = False | |
start = time.time() | |
while time.time() < start + timeout: | |
msg = master.recv_match() | |
if msg is not None: | |
if msg.get_type() == "COMMAND_ACK" and msg.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN and msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: | |
print(" OK") | |
verified = True | |
break | |
time.sleep(0.01) | |
if not verified: | |
print(" FAIL!") | |
exit(1) | |
master.close() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment