mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
479 lines
18 KiB
Python
Executable File
479 lines
18 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
'''
|
|
Command-line utility to handle comms to gimbal
|
|
'''
|
|
import time, os, sys, argparse, time, json
|
|
import setup_mavlink, setup_factory_pub
|
|
|
|
from firmware_helper import load_firmware, append_checksum
|
|
from firmware_loader import load_binary, start_bootloader
|
|
from firmware_loader import Results as loader_results
|
|
import setup_validate, setup_param, setup_comutation
|
|
from setup_comutation import Results as calibration_results
|
|
|
|
# Optional imports
|
|
try:
|
|
import setup_run
|
|
import setup_home
|
|
import setup_factory_private
|
|
import firmware_git_tools
|
|
except ImportError:
|
|
setup_run = None
|
|
setup_home = None
|
|
setup_factory_private = None
|
|
firmware_git_tools = None
|
|
|
|
def handle_file(args, link):
|
|
fileExtension = str(args.file).split(".")[-1].lower()
|
|
if fileExtension == 'param':
|
|
setup_param.load_param_file(args.file, link)
|
|
elif fileExtension == 'json':
|
|
with open(args.file) as f:
|
|
params = json.load(f)
|
|
setup_param.restore_params(link, params)
|
|
print("Gimbal parameters restored.")
|
|
elif fileExtension == 'ax':
|
|
# Prepare the binary to load from the compressed .ax file
|
|
print('Application firmware_file: %s' % args.file)
|
|
firmware = load_firmware(args.file)
|
|
binary, checksum = append_checksum(firmware['binary'])
|
|
print('Checksum: 0x%04X' % checksum)
|
|
|
|
# Start the bootloader
|
|
result = start_bootloader(link)
|
|
if result == loader_results.NoResponse:
|
|
print("No response from gimbal, exiting.")
|
|
sys.exit(1)
|
|
elif result == loader_results.InBoot:
|
|
print('Target already in bootloader mode')
|
|
elif result == loader_results.Restarting:
|
|
print("Restarted target in bootloader mode")
|
|
|
|
def loaderProgressCallback(uploaded_kb, total_kb, percentage):
|
|
if percentage == 0:
|
|
sys.stdout.write("\rErasing flash...")
|
|
else:
|
|
sys.stdout.write("\rUploading %2.2fkB of %2.2fkB - %d%%" % (uploaded_kb, total_kb, percentage))
|
|
# The flush is required to refresh the screen on Ubuntu
|
|
sys.stdout.flush()
|
|
|
|
def bootloaderVersionCallback(major, minor):
|
|
print('Bootloader Ver %i.%i' % (major, minor))
|
|
|
|
# Load the binary using the bootloader
|
|
result = load_binary(binary, link, bootloaderVersionCallback=bootloaderVersionCallback, progressCallback=loaderProgressCallback)
|
|
sys.stdout.write("\n")
|
|
sys.stdout.flush();
|
|
if result == loader_results.Success:
|
|
print("Upload successful")
|
|
|
|
# If the script is being run on a Solo, make sure custom gains are diabled
|
|
if setup_factory_private is None:
|
|
setup_mavlink.wait_for_gimbal_message(link)
|
|
setup_param.set_use_custom_gains(link, 0)
|
|
elif result == loader_results.NoResponse:
|
|
print("No response from gimbal, exiting.")
|
|
sys.exit(1)
|
|
elif result == loader_results.Timeout:
|
|
print("Timeout")
|
|
sys.exit(1)
|
|
else:
|
|
print("Unknown error while finishing bootloading")
|
|
sys.exit(1)
|
|
else:
|
|
print("File type not supported")
|
|
sys.exit(1)
|
|
return
|
|
|
|
def calibrationProgressCallback(axis, progress, status):
|
|
text = "\rCalibrating %s - progress %d%% - %s " % (axis, progress, status)
|
|
sys.stdout.write(text)
|
|
# The flush is required to refresh the screen on Ubuntu
|
|
sys.stdout.flush()
|
|
|
|
def eraseCalibration(link):
|
|
print('Clearing old calibration values...')
|
|
setup_comutation.resetCalibration(link)
|
|
print('Calibration values cleared')
|
|
|
|
def runCalibration(link):
|
|
result = setup_comutation.calibrate(link, calibrationProgressCallback)
|
|
sys.stdout.write("\n")
|
|
sys.stdout.flush()
|
|
|
|
if result == calibration_results.ParamFetchFailed:
|
|
print("Failed to get calibration parameters from the gimbal")
|
|
return
|
|
elif result == calibration_results.CommsFailed:
|
|
print("Gimbal failed to communicate calibration progress")
|
|
return
|
|
|
|
# These results require a reset of the Gimbal
|
|
if result == calibration_results.Success:
|
|
print("Calibration successful!")
|
|
return
|
|
elif result == calibration_results.CalibrationExists:
|
|
print("A calibration already exists, erase current calibration first (-e)")
|
|
return
|
|
elif result == calibration_results.PitchFailed:
|
|
print("Pitch calibration failed")
|
|
return
|
|
elif result == calibration_results.RollFailed:
|
|
print("Roll calibration failed")
|
|
return
|
|
elif result == calibration_results.YawFailed:
|
|
print("Yaw calibration failed")
|
|
return
|
|
|
|
# Reset the gimbal
|
|
print("Rebooting Gimbal")
|
|
setup_mavlink.reset_gimbal(link)
|
|
|
|
# Get the calibration data
|
|
if result == calibration_results.Success:
|
|
calibration = setup_comutation.getAxisCalibrationValues(link)
|
|
if calibration:
|
|
print("")
|
|
for axis in ['pitch', 'roll', 'yaw']:
|
|
print("%s: slope=%f intercept=%f" % (axis, calibration[axis]['slope'], calibration[axis]['intercept']))
|
|
else:
|
|
print("Error getting calibration values from the gimbal")
|
|
return
|
|
|
|
def printValidation(link):
|
|
valid = setup_validate.validate_version(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Version \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Version \t- FAIL - please update to latest gimbal software")
|
|
else:
|
|
print("Version \t- ERROR")
|
|
|
|
valid = setup_validate.validate_serial_number(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Serial Number \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Serial Number \t- FAIL - Serial number was not set (--serialnumber SERIALNUMBER)")
|
|
else:
|
|
print("Serial Number \t- ERROR")
|
|
|
|
valid = setup_validate.validate_date(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Assembly date \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Assembly date \t- FAIL - assembly date was not set on the factory (--date)")
|
|
else:
|
|
print("Assembly date \t- ERROR")
|
|
|
|
valid = setup_validate.validate_comutation(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Comutation \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Comutation \t- FAIL - please redo the comutation calibration (-c)")
|
|
else:
|
|
print("Comutation \t- ERROR")
|
|
|
|
valid = setup_validate.validate_joints(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Joints \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Joints \t- FAIL - redo joint calibration (-j)")
|
|
else:
|
|
print("Joints \t- ERROR")
|
|
|
|
valid = setup_validate.validate_gyros(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Gyros \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Gyros \t- FAIL - redo gyro calibration (-g)")
|
|
else:
|
|
print("Gyros \t- ERROR")
|
|
|
|
valid = setup_validate.validate_accelerometers(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Accelerometer\t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Accelerometer\t- FAIL - redo accelerometer calibration (-a)")
|
|
else:
|
|
print("Accelerometer\t- ERROR")
|
|
|
|
valid = setup_validate.validate_gains(link)
|
|
if valid == setup_validate.Results.Pass:
|
|
print("Gains \t- PASS")
|
|
elif valid == setup_validate.Results.Fail:
|
|
print("Gains \t- WARNING - custom gains are enabled (disable with --customgains=0)")
|
|
else:
|
|
print("Gains \t- ERROR")
|
|
|
|
# Main method when called directly
|
|
def command_interface():
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("file", nargs='?', help="parameter or firmware file to be loaded into the gimbal", default=None)
|
|
parser.add_argument("-p", "--port", help="Serial port or device used for MAVLink bootloading", default=None)
|
|
parser.add_argument("--show", help="Show all gimbal parameters", action='store_true')
|
|
parser.add_argument("--save", help="Save gimbal parameters to a file", action='store_true')
|
|
parser.add_argument("-v", "--validate", help="Check gimbal parameters to see if they have valid values", action='store_true')
|
|
parser.add_argument("-d", "--defaults", help="Reset gimbal parameters to default values", action='store_true')
|
|
parser.add_argument("-r","--reboot", help="Reboot the gimbal", action='store_true')
|
|
parser.add_argument("--eraseapp", help="Erase the application", action='store_true')
|
|
parser.add_argument("-c", "--calibrate", help="Run the comutation setup", action='store_true')
|
|
parser.add_argument("-f", "--forcecal", help="Force the comutation setup", action='store_true')
|
|
parser.add_argument("-e", "--erase", help="Erase calibration values", action='store_true')
|
|
parser.add_argument("--customgains", help="Enable the use of custom gains (0 to disable, 1 to enable)", type=int)
|
|
parser.add_argument("--charging", help="Set camera charging (0 to disable, 1 to enable)", type=int)
|
|
|
|
# Optional commands (not included with sololink tools)
|
|
if setup_run:
|
|
parser.add_argument("--run", help="run a quick test of the gimbal", action='store_true')
|
|
parser.add_argument("--align", help="move the gimbal to the home position", action='store_true')
|
|
parser.add_argument("--stop", help="Hold the gimbal at the current position", action='store_true')
|
|
parser.add_argument("--wobble", help="Wobble fixture test", action='store_true')
|
|
parser.add_argument("--timeout", help="timeout for action", type=int)
|
|
parser.add_argument("--testloop", help="run a loop of 'run', 'align' and 'wobble' tests", type=str)
|
|
parser.add_argument("--lifetest", help="run the gimbal life test", action='store_true')
|
|
parser.add_argument("--limp", help="disable position hold mode", action='store_true')
|
|
parser.add_argument("--nolimp", help="enable position hold mode", action='store_true')
|
|
parser.add_argument("--hard", help="enable low torque mode", action='store_true')
|
|
parser.add_argument("--soft", help="disable low torque mode", action='store_true')
|
|
if setup_home:
|
|
parser.add_argument("-j", "--jointcalibration", help="Calibrate joint angles", action='store_true')
|
|
parser.add_argument("-g", "--gyrocalibration", help="Calibrate gyros", action='store_true')
|
|
parser.add_argument("-a", "--accelcalibration", help="Calibrate accelerometers", action='store_true')
|
|
parser.add_argument("-x", "--staticcal", help="Calibrate all static home values", action='store_true')
|
|
if setup_factory_private:
|
|
parser.add_argument("--date", help="Setup assembly date", action='store_true')
|
|
parser.add_argument("--serialnumber", help="Setup gimbal serial number", type=int)
|
|
parser.add_argument("--factoryreset", help="Reset gimbal factory parameters to default", action='store_true')
|
|
parser.add_argument("--fullreset", help="Clear all gimbal parameters and program memory", action='store_true')
|
|
|
|
args = parser.parse_args()
|
|
|
|
# Open the serial port
|
|
port, link = setup_mavlink.open_comm(args.port)
|
|
print("Connecting via port %s" % port)
|
|
|
|
if link is None:
|
|
print("Failed to open port %s" % port)
|
|
sys.exit(1)
|
|
|
|
# Send a heartbeat first to wake up the interface, because mavlink
|
|
link.heartbeat_send(0, 0, 0, 0, 0)
|
|
|
|
msg = setup_mavlink.get_any_gimbal_message(link)
|
|
if msg:
|
|
if setup_mavlink.is_bootloader_message(msg) and not args.file:
|
|
print("Gimbal in bootloader, only firmware loading is available")
|
|
sys.exit(0)
|
|
else:
|
|
# If the gimbal is using a different mavlink definitions version to
|
|
# what this script is using, try detecting the gimbal using a
|
|
# standard mavlink message that will not have changed.
|
|
# Standard mavlink heartbeats could be used, but are usually not
|
|
# propogated through the pixhawk in Solo
|
|
ver = setup_factory_pub.read_software_version(link, timeout=2)
|
|
if ver:
|
|
print("Possible mismatch in gimbal mavlink definitions, continuing anyway")
|
|
else:
|
|
print("No gimbal messages received, exiting.")
|
|
sys.exit(1)
|
|
|
|
if args.file:
|
|
handle_file(args, link)
|
|
return
|
|
|
|
if args.eraseapp:
|
|
result = start_bootloader(link)
|
|
if result == loader_results.NoResponse:
|
|
print("No response from gimbal, exiting.")
|
|
elif result == loader_results.InBoot:
|
|
print("Application already erased")
|
|
elif result == loader_results.Restarting:
|
|
print("Application erased")
|
|
return
|
|
|
|
if args.calibrate:
|
|
runCalibration(link)
|
|
return
|
|
|
|
if args.forcecal:
|
|
eraseCalibration(link)
|
|
# TODO: Check if this timeout is necessary
|
|
time.sleep(5)
|
|
runCalibration(link)
|
|
return
|
|
|
|
if args.show:
|
|
params = setup_validate.show(link)
|
|
if params:
|
|
print(json.dumps(params, sort_keys=True, indent=4, separators=(',', ': ')))
|
|
else:
|
|
print("Error fetching parameters from gimbal")
|
|
return
|
|
|
|
if args.save:
|
|
params = setup_validate.show(link)
|
|
if params:
|
|
if not os.path.isdir('logs'):
|
|
os.makedirs('logs')
|
|
|
|
if params['serial_number'] != None and params['serial_number'] != '':
|
|
filePath = os.path.join('logs', '%s.json' % params['serial_number'])
|
|
with open(filePath, 'w') as f:
|
|
json.dump(params, f, sort_keys=True, indent=4, separators=(',', ': '))
|
|
print("Gimbal prameters saved to %s" % filePath)
|
|
else:
|
|
print("Gimbal serial number not set, prameters not saved.")
|
|
else:
|
|
print("Error fetching parameters from gimbal")
|
|
return
|
|
|
|
if args.validate:
|
|
print("Validating gimbal parameters...")
|
|
printValidation(link)
|
|
return
|
|
|
|
if args.defaults:
|
|
setup_validate.restore_defaults(link)
|
|
print('Parameters restored to default values')
|
|
return
|
|
|
|
if args.reboot:
|
|
if not setup_mavlink.reset_gimbal(link):
|
|
print('Failed to reboot')
|
|
return
|
|
|
|
if args.erase:
|
|
eraseCalibration(link)
|
|
return
|
|
|
|
if args.customgains == 1 or args.customgains == 0:
|
|
setup_param.set_use_custom_gains(link, args.customgains)
|
|
return
|
|
|
|
if args.charging == 1 or args.charging == 0:
|
|
setup_param.charging(link, args.charging)
|
|
return
|
|
|
|
if setup_run:
|
|
if args.run:
|
|
setup_run.runTest(link, 'run')
|
|
return
|
|
|
|
if args.align:
|
|
setup_run.runTest(link, 'align')
|
|
return
|
|
|
|
if args.wobble:
|
|
setup_run.runTest(link, 'wobble', timeout=args.timeout)
|
|
return
|
|
|
|
if args.testloop:
|
|
if args.testloop in ['run', 'align', 'wobble']:
|
|
setup_run.runTestLoop(link, args.testloop, timeout=args.timeout)
|
|
return
|
|
else:
|
|
print("Unknown test: %s" % args.testloop)
|
|
|
|
if args.lifetest:
|
|
setup_run.runLifeTest(link)
|
|
return
|
|
|
|
if args.stop:
|
|
setup_run.stop(link)
|
|
return
|
|
|
|
if args.limp:
|
|
setup_param.pos_hold_disable(link)
|
|
return
|
|
|
|
if args.nolimp:
|
|
setup_param.pos_hold_enable(link)
|
|
return
|
|
|
|
if args.hard:
|
|
setup_param.low_torque_mode(link, False)
|
|
return
|
|
|
|
if args.soft:
|
|
setup_param.low_torque_mode(link, True)
|
|
return
|
|
|
|
if setup_home:
|
|
if args.jointcalibration or args.staticcal:
|
|
print('Calibrating home position')
|
|
offsets = setup_home.calibrate_joints(link)
|
|
if offsets:
|
|
print('Calibrated home position')
|
|
else:
|
|
print('Failed to calibrate home position')
|
|
if not args.staticcal:
|
|
return
|
|
|
|
if args.gyrocalibration or args.staticcal:
|
|
print('Calibrating gyro offsets')
|
|
offsets = setup_home.calibrate_gyro(link)
|
|
if offsets:
|
|
print('Calibrated gyro offsets')
|
|
else:
|
|
print('Failed to calibrate gyro offsets')
|
|
return
|
|
|
|
if args.accelcalibration:
|
|
setup_home.calibrate_accel(link)
|
|
return
|
|
|
|
if setup_factory_private:
|
|
if args.date:
|
|
timestamp = setup_factory_private.set_assembly_date(link)
|
|
print("Assembly time set to %s" % time.ctime(timestamp))
|
|
return
|
|
|
|
if args.serialnumber is not None:
|
|
serial = setup_factory_private.set_serial_number_3dr(link, args.serialnumber)
|
|
print("Serial number set to %s" % serial)
|
|
return
|
|
|
|
if args.factoryreset:
|
|
serial = setup_factory_private.reset(link)
|
|
print("Facroty parameters cleared")
|
|
return
|
|
|
|
if args.fullreset:
|
|
setup_factory_private.full_reset(link)
|
|
print("Full reset command sent")
|
|
return
|
|
|
|
# Default command is to return the software version number
|
|
if firmware_git_tools:
|
|
os_git_command = firmware_git_tools.osGitCommand()
|
|
print("Tools version: %s"%(firmware_git_tools.gitIdentity(os_git_command)))
|
|
ver = setup_factory_pub.read_software_version(link, timeout=4)
|
|
if ver != None:
|
|
major, minor, rev = ver[0], ver[1], ver[2]
|
|
print("Software version: v%i.%i.%i" % (major, minor, rev))
|
|
|
|
serial_number = setup_factory_pub.get_serial_number(link)
|
|
if serial_number != None:
|
|
if serial_number == '':
|
|
print("Serial number: not set")
|
|
else:
|
|
print("Serial number: " + serial_number)
|
|
else:
|
|
print("Serial number: unknown")
|
|
|
|
asm_time = setup_factory_pub.get_assembly_time(link)
|
|
if asm_time != None:
|
|
if asm_time > 0:
|
|
print("Assembled time: " + time.ctime(asm_time))
|
|
else:
|
|
print("Assembly time: not set")
|
|
else:
|
|
print("Assembly time: unknown")
|
|
else:
|
|
print("Software version: unknown")
|
|
|
|
if __name__ == '__main__':
|
|
command_interface()
|
|
sys.exit(0)
|