mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-05-02 07:34:33 +02:00
73 lines
1.9 KiB
Python
Executable File
73 lines
1.9 KiB
Python
Executable File
#!/usr/bin/env python
|
|
#
|
|
# valid fields for each axis: stick_id, direction, expo (optional, defaults to 2)
|
|
#
|
|
|
|
import sys, os, struct, ConfigParser
|
|
import serial, slip
|
|
|
|
ARTOO_BAUD = 115200
|
|
DEFAULT_EXPO = 2
|
|
|
|
ARTOO_STORED_PARAMS_ID = chr(0xb)
|
|
ARTOO_CFG_STICK_AXES_ID = chr(0xf)
|
|
|
|
def wait_for_params(slipdev):
|
|
while True:
|
|
slipdev.write("".join([ARTOO_STORED_PARAMS_ID]))
|
|
pkt = slipdev.read()
|
|
pkt = "".join(pkt)
|
|
if pkt[0] == ARTOO_STORED_PARAMS_ID:
|
|
return pkt[1:]
|
|
|
|
def get_field(cfg, axis, field, valid_range = None):
|
|
# if not specified, leave uninitialized
|
|
if not cfg.has_option(axis, field):
|
|
return 0xff
|
|
|
|
f = cfg.getint(axis, field)
|
|
if valid_range:
|
|
if f not in valid_range:
|
|
raise ValueError("bad value for %s" % field)
|
|
|
|
return f
|
|
|
|
def validate_and_pack_axis_info(cfg, axis):
|
|
stick_id = get_field(cfg, axis, 'mapped_stick_id', range(5))
|
|
# NB: direction - non-zero is Forward, 0 is Reverse
|
|
direction = get_field(cfg, axis, 'direction', [0, 1])
|
|
expo = get_field(cfg, axis, 'expo')
|
|
|
|
return struct.pack("<BBBBI", stick_id, direction, expo, 0, 0xffffffff)
|
|
|
|
|
|
def mainloop(serialpath, cfg):
|
|
|
|
serialport = serial.Serial(serialpath, ARTOO_BAUD)
|
|
slipdev = slip.SlipDevice(serialport)
|
|
|
|
payload = ""
|
|
for axis in ["stick-0", "stick-1", "stick-2", "stick-3", "stick-4", "stick-5"]:
|
|
payload += validate_and_pack_axis_info(cfg, axis)
|
|
|
|
slipdev.write(ARTOO_CFG_STICK_AXES_ID + payload)
|
|
print "\nverifying config..."
|
|
params = wait_for_params(slipdev)
|
|
# XXX: compare params...
|
|
print "Calibration complete, values stored."
|
|
|
|
|
|
#
|
|
# main
|
|
#
|
|
|
|
if len(sys.argv) < 3:
|
|
print "usage: stick-axis-cfg.py /dev/ttyMyDev configfile.cfg"
|
|
sys.exit(1)
|
|
|
|
config = ConfigParser.ConfigParser()
|
|
config.optionxform = str # don't lower case all option names
|
|
config.read(os.path.join(os.getcwd(), sys.argv[2]))
|
|
|
|
mainloop(sys.argv[1], config)
|