def __init__(self, inverter_id, listener):
self._id = inverter_id
self._listener = listener
self._max_retry_count = MAX_RETRIES
self._max_command_attempts = 2
self._stale_time_exceeded = False
self._stale_count_exceeded = False
self._command_delay = 1.0
self._set_param_delay = 0.1
self._get_param_delay = 0.1
self._status = None
self.state_transitions = {
# {current_state : {target_state1: command1}, ...}
inverter_types.kInverterStatusRunning: {
inverter_types.kInverterStatusStopped:
flags.kGroundPowerCommandStop,
},
inverter_types.kInverterStatusStopped: {
inverter_types.kInverterStatusRunning:
flags.kGroundPowerCommandStart,
},
inverter_types.kInverterStatusEStopped: {},
inverter_types.kInverterStatusFaulted: {},
inverter_types.kInverterStatusInit: {},
}
self._set_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerSetParam'], timeout=0.2)
self._ack_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerAckParam'], timeout=0.2)
self._get_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerGetParam'], timeout=0.2)
self._command_aio_client = aio.AioClient(
['kMessageTypeGroundPowerCommand'], timeout=0.2)
def __init__(self, *args, **kwargs):
cmd_client.WingCommandClient.__init__(self, *args, **kwargs)
self._set_param_aio_client = aio.AioClient(
['kMessageTypeFaaLightSetParam'], timeout=0.1)
# The long range radio requires at least 2x160 ms for a complete command-
# response cycle.
self._ack_param_aio_client = aio.AioClient(
['kMessageTypeFaaLightAckParam'], timeout=0.35)
self._get_param_aio_client = aio.AioClient(
['kMessageTypeFaaLightGetParam'], timeout=0.1)
def testStdio(self):
source = aio.AioClient(['kMessageTypeMotorStatus',
'kMessageTypeStdio'])
sink = aio.AioClient(['kMessageTypeStdio'], timeout=0.01)
source.Send('test', 'kMessageTypeStdio', 'kAioNodeMotorSbo')
(_, header, string) = sink.Recv()
self.assertEqual(header.source, network_config.kAioNodeMotorSbo)
self.assertEqual(header.type, network_config.kMessageTypeStdio)
self.assertEqual(string, 'test')
source.Close()
sink.Close()
def testSourceFiltering(self):
source = aio.AioClient(['kMessageTypeMotorStatus',
'kMessageTypeStdio'])
sink = aio.AioClient(['kMessageTypeStdio'],
allowed_sources=['kAioNodeMotorSti'],
timeout=0.01)
source.Send('test', 'kMessageTypeStdio', 'kAioNodeMotorSbo')
with self.assertRaises(socket.timeout):
_ = sink.Recv()
source.Close()
sink.Close()
def main():
if len(sys.argv) != 2:
print textwrap.dedent("""
Inspect the switch routing table of an AIO node. Currently only access
switches are supported.
Usage: route_dump <node_name>"""[1:])
sys.exit(-1)
aio_client = aio.AioClient(
['kMessageTypeDumpRoutesRequest', 'kMessageTypeDumpRoutesResponse'],
timeout=1)
request_msg = pack_avionics_messages.DumpRoutesRequestMessage()
request_msg.target = aio_node_helper.Value(sys.argv[1])
aio_client.Send(request_msg, 'kMessageTypeDumpRoutesRequest',
'kAioNodeOperator')
responses = []
while True:
try:
(_, header, msg) = aio_client.Recv()
mtype = message_type_helper.ShortName(header.type)
if mtype == 'DumpRoutesResponse' and header.source == request_msg.target:
responses.append(msg)
except socket.timeout:
break
def _SortKey(msg):
return '%3X%s' % (msg.entry.vlan_id,
_MacToString(msg.entry.ethernet_address))
for msg in sorted(responses, key=_SortKey):
_FormatResponse(request_msg.target, msg)
def __init__(self,
source,
num_pole_pairs,
sensor_ratio,
save_data,
parent=None):
"""Initializes a listener for position sensor data.
Args:
source: AIO node name for motor to receive packets from.
num_pole_pairs: Number of pole pairs for given motor.
sensor_ratio: Ratio of position sensor angle to motor electrical angle.
save_data: A boolean indicating whether or not to save current run data.
parent: An optional parent argument for QtCore.QThread.
"""
QtCore.QThread.__init__(self, parent)
self.should_exit = False
self._num_pole_pairs = num_pole_pairs
self._sensor_ratio = sensor_ratio
self._save_data = save_data
self._aio_client = aio.AioClient(['kMessageTypeMotorCalibration'],
allowed_sources=[source],
timeout=0.6)
self._noise_packets = []
self._angle_packets = []
def UpdateVersion(self, timeout=1):
"""Get AIO version from header of any packet from select_node."""
sources = [self.select_node.enum_name]
types = aio.message_type_helper.Names()
self.dialog_ui.Info('Watching for messages...({}s)'.format(timeout),
title='Version Checker')
try:
client = aio.AioClient(types,
timeout=timeout,
allowed_sources=sources)
_, header, _ = client.Recv(accept_invalid=True)
except socket.timeout as e:
self.version = None
return -1, '', str(e)
except socket.error as e:
if str(e) == 'timed out':
self.version = None
return -1, '', str(e)
else:
raise
else:
self.version = str(header.version)
finally:
client.Close()
return 0, '', ''
def __init__(self,
downsample,
buffer_size,
source,
data_queue,
parent=None):
"""Initialize a StatusListener.
Args:
downsample: An integer specifying the subsample ratio of the data that is
passed back via the queue.
buffer_size: An integer specifying how many data points are added to the
queue before the thread emits its has_data signal.
source: A string specifying which AIO node to listen to.
data_queue: A Queue.Queue used to pass data back to the instantiating
thread in a thread-safe manner.
parent: An optional parent argument for QtCore.QThread.
"""
QtCore.QThread.__init__(self, parent)
self.should_exit = False
self._downsample = downsample
self._buffer_size = buffer_size
self._data_queue = data_queue
self._aio_client = aio.AioClient(['kMessageTypeMotorStatus'],
allowed_sources=[source],
timeout=0.2)
def __init__(self,
allowed_sources,
message_type,
message_template,
period,
history,
parent=None):
QtCore.QThread.__init__(self, parent)
QtCore.QMutex.__init__(self)
self.should_exit = False
self._half_size = int(numpy.ceil(history / period))
self._buffer_size = 2 * self._half_size
self._period = period
self._head = 0
self._timestamp = time.time()
self._source_map = {
aio_node_helper.Value(x): i
for i, x in enumerate(allowed_sources)
}
self._aio_client = aio.AioClient(message_types=[message_type],
allowed_sources=allowed_sources,
timeout=0.2)
self.time = period * numpy.arange(-self._half_size + 1, 1)
self._data = [None] * len(allowed_sources)
self._derived_params = collections.OrderedDict()
message_dict = ctype_util.CTypeToPython(message_template)
for i in range(len(self._data)):
self._data[i] = self._InitBuffers(message_dict)
def __init__(self, *args, **kwargs):
cmd_client.WingCommandClient.__init__(self, *args, **kwargs)
self._servos_selected = set()
self._listener = None
self._runner = None
self._set_state_aio_client = aio.AioClient(
['kMessageTypeServoSetState'], timeout=0.1)
self._set_param_aio_client = aio.AioClient(
['kMessageTypeServoSetParam'], timeout=0.1)
# The long range radio requires at least 2x160 ms for a complete command-
# response cycle.
self._ack_param_aio_client = aio.AioClient(
['kMessageTypeServoAckParam'], timeout=0.35)
self._get_param_aio_client = aio.AioClient(
['kMessageTypeServoGetParam'], timeout=0.1)
def _SendClearErrors():
"""Sends 'clear errors' command to PLC AIO node."""
msg = pack_avionics_messages.GroundStationDetwistSetStateMessage()
msg.state_command = actuator_types.kActuatorStateCommandClearErrors
messages = ['kMessageTypeGroundStationDetwistSetState']
client = aio.AioClient(messages)
client.Send(msg,
'kMessageTypeGroundStationDetwistSetState',
'kAioNodeOperator')
def testMotorStatusMessage(self):
source = aio.AioClient(['kMessageTypeMotorStatus',
'kMessageTypeStdio'])
sink = aio.AioClient(['kMessageTypeMotorStatus'], timeout=0.01)
before = pack_avionics_messages.MotorStatusMessage()
before.id = 3.14
before.omega = 5.16
source.Send(before, 'kMessageTypeMotorStatus', 'kAioNodeMotorSbo')
(_, header, after) = sink.Recv()
self.assertEqual(header.source, network_config.kAioNodeMotorSbo)
self.assertEqual(header.type, network_config.kMessageTypeMotorStatus)
self.assertEqual(before.id, after.id)
self.assertEqual(before.omega, after.omega)
source.Close()
sink.Close()
def _SendReferenceCommand(ref_position):
"""Sends 'reference' command to PLC AIO node."""
msg = pack_avionics_messages.GroundStationPlcOperatorMessage()
msg.command.detwist_cmd = pack_avionics_messages.kDetwistCommandReference
msg.command.detwist_position = ref_position
messages = ['kMessageTypeGroundStationPlcOperator']
client = aio.AioClient(messages)
client.Send(msg,
'kMessageTypeGroundStationPlcOperator',
'kAioNodeOperator')
def Main():
# Join multicast group to receive UDP packets from torque cell.
futek_client = OpenTorqueCellSocket()
# Struct format for unpacking received data.
data_struct = struct.Struct('f f f')
# AIO Client to send out AIO packets of torque data.
torque_cell_aio_client = aio.AioClient(['kMessageTypeTorqueCell'])
while True:
RecvTorqueSendAio(futek_client, data_struct, torque_cell_aio_client)
def CheckConsole(self):
"""Display the Stdio and SelfTest messages from select_node using dialog."""
sources = [self.select_node.enum_name]
types = ['kMessageTypeStdio', 'kMessageTypeSelfTest']
self.dialog_ui.Info('Watching for Stdio messages...(3s)',
title='Console Checker')
try:
client = aio.AioClient(types, timeout=3, allowed_sources=sources)
_, _, message = client.Recv(accept_invalid=True)
self.dialog_ui.Message(getattr(message, 'text', repr(message)),
title='Console Message')
except socket.timeout, e:
return -1, '', 'No Stdio messages found.'
def __init__(self, source, data_queue, parent=None):
"""Initialize an AdcDataListener.
Args:
source: A string specifying which AIO node to listen to.
data_queue: A Queue.Queue used to pass data back to the instantiating
thread in a thread-safe manner.
parent: An optional parent argument for QtCore.QThread.
"""
QtCore.QThread.__init__(self, parent)
self.should_exit = False
self._data_queue = data_queue
self._aio_client = aio.AioClient(['kMessageTypeMotorAdcLog'],
allowed_sources=[source],
timeout=0.1)
def main():
aio_client = aio.AioClient(
['kMessageTypeLatencyProbe', 'kMessageTypeLatencyResponse'], timeout=1)
while True:
probe_msg = pack_avionics_messages.LatencyProbeMessage()
probe_msg.timestamp = _GetUSec()
aio_client.Send(probe_msg, 'kMessageTypeLatencyProbe',
'kAioNodeOperator')
while True:
try:
(addr, header, msg) = aio_client.Recv()
now = _GetUSec()
mtype = message_type_helper.ShortName(header.type)
if mtype == 'LatencyResponse':
print '%s: %dus' % (addr, _SubUSec(now, msg.timestamp))
except socket.timeout:
break
def __init__(self, *args, **kwargs):
cmd_client.WingCommandClient.__init__(self, *args, **kwargs)
self._model = 'equinox'
self._ground_power_listener = None
self._SetTargets('', set_defaults=True)
self._set_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerSetParam'], timeout=0.2)
self._ack_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerAckParam'], timeout=0.2)
self._get_param_aio_client = aio.AioClient(
['kMessageTypeGroundPowerGetParam'], timeout=0.2)
self._command_aio_client = aio.AioClient(
['kMessageTypeGroundPowerCommand'], timeout=0.2)
self._set_net_load_aio_client = aio.AioClient(
['kMessageTypeLoadbankSetLoad'], timeout=0.2)
self._set_loadbank_state_aio_client = aio.AioClient(
['kMessageTypeLoadbankSetState'], timeout=0.2)
self._ack_net_load_aio_client = aio.AioClient(
['kMessageTypeLoadbankAckParam'], timeout=0.2)
self._ack_loadbank_state_aio_client = aio.AioClient(
['kMessageTypeLoadbankStateAckParam'], timeout=0.2)
def DetectAio(timeout=1.1):
"""Detect AIO nodes on the network, present all options if none detected."""
sources = aio.aio_node_helper.Names()
types = aio.message_type_helper.Names()
client = aio.AioClient(types, timeout=0.1, allowed_sources=sources)
ip_list = []
version_list = []
timer_start = time.time()
while time.time() - timer_start < timeout:
try:
ip, header, _ = client.Recv(accept_invalid=True)
ip_list.append(ip)
version_list.append(header.version)
except socket.error:
pass
client.Close()
if ip_list and version_list:
# De-duplication using set conversion.
ip_tuple, version_tuple = zip(*set(zip(ip_list, version_list)))
return tuple([IpToAioNode(ip) for ip in ip_tuple]), version_tuple
return tuple(), tuple()
def __init__(self, message_types, **aio_client_kwargs):
super(AioThread, self).__init__()
self._client = aio.AioClient(message_types, **aio_client_kwargs)
self._should_exit = False
def testIsDuplicate(self):
sink = aio.AioClient(['kMessageTypeMotorStatus'], timeout=0.01)
header = aio_header.AioHeader()
header.version = aio_version.AIO_VERSION
header.source = network_config.kAioNodeMotorSbo
header.type = network_config.kMessageTypeMotorStatus
header.sequence = 0
header.timestamp = 0
last_time = aio_header.AIO_EXPIRATION_TIME_US * 1e-6
# Test first update (we have no knowledge of the initialization state).
header.sequence = 100
self.assertFalse(sink.IsDuplicate(header, '', last_time))
header.sequence = (100 + aio_header.AIO_ACCEPTANCE_WINDOW) % 2**16
self.assertFalse(sink.IsDuplicate(header, '', last_time))
# Explicitly set last update.
last_seq = 1
SetLastSeqTime(sink, header.source, header.type, last_seq, last_time)
# Test duplicate sequence number.
header.sequence = last_seq
self.assertTrue(sink.IsDuplicate(header, '', last_time))
# Test previous sequence number.
header.sequence = (last_seq + 2**16 - 1) % 2**16
self.assertTrue(sink.IsDuplicate(header, '', last_time))
# Test next sequence number.
header.sequence = (last_seq + 1) % 2**16
self.assertFalse(sink.IsDuplicate(header, '', last_time))
# Test sequence number acceptance window.
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW + 1) % 2**16
self.assertTrue(sink.IsDuplicate(header, '', last_time))
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW) % 2**16
self.assertFalse(sink.IsDuplicate(header, '', last_time))
# Test sequence number rollover.
last_seq = 2**16 - 1
SetLastSeqTime(sink, header.source, header.type, last_seq, last_time)
header.sequence = (last_seq + 1) % 2**16
self.assertFalse(sink.IsDuplicate(header, '', last_time))
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW + 1) % 2**16
self.assertTrue(sink.IsDuplicate(header, '', last_time))
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW) % 2**16
self.assertFalse(sink.IsDuplicate(header, '', last_time))
# Test duplicate after expiration time.
header.sequence = last_seq
SetLastSeqTime(sink, header.source, header.type, last_seq, last_time)
self.assertTrue(sink.IsDuplicate(header, '', last_time))
cur_time = last_time + aio_header.AIO_EXPIRATION_TIME_US * 1e-6
self.assertTrue(sink.IsDuplicate(header, '', cur_time - 1e-6))
self.assertFalse(sink.IsDuplicate(header, '', cur_time))
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW + 1) % 2**16
self.assertTrue(sink.IsDuplicate(header, '', last_time))
header.sequence = (last_seq + aio_header.AIO_ACCEPTANCE_WINDOW + 1) % 2**16
self.assertTrue(sink.IsDuplicate(header, '', cur_time - 1e-6))
self.assertFalse(sink.IsDuplicate(header, '', cur_time))
# Clean-up.
sink.Close()
target_node = config.GetAioNode(FLAGS.node)
if target_node.label_name != 'core_switch':
raise ValueError('Only core switches supported in switch_client.')
if (target_node.snake_name != 'cs_gs_a'
and target_node.snake_name != 'cs_gs_b'
and not FLAGS.i_am_not_breaking_the_wing):
raise ValueError(
'Can only disable ports on cs_gs_a or cs_gs_b by default.'
' If you know what you are doing please specify '
'--i_am_not_breaking_the_wing to modify the wing core '
'switches. Incorrect usage can break the wing (requiring '
'power cycle to restore connection).')
if (disable_port_mask & (1 << 25)) != 0:
raise ValueError('Cannot disable the command center port.')
client_sender = aio.AioClient(['kMessageTypeCoreSwitchConnectionSelect'],
allowed_sources=['kAioNodeOperator'])
client_receiver = aio.AioClient(['kMessageTypeCoreSwitchStatus'],
allowed_sources=[target_node.enum_name],
timeout=1.0)
message = pack_avionics_messages.CoreSwitchConnectionSelectMessage()
message.target = target_node.enum_value
message.disable_port_mask = disable_port_mask
end_time = time.time() + 10 # Give them 10 seconds to switch.
while time.time() < end_time:
client_sender.Send(message, 'kMessageTypeCoreSwitchConnectionSelect',
'kAioNodeOperator')
try:
(_, _, received_message) = client_receiver.Recv()
if received_message.disabled_port_mask == message.disable_port_mask:
def __init__(self, *args, **kwargs):
wx.App.__init__(self, *args, **kwargs)
self._seq = 0
self._aio_client = aio.AioClient(
['kMessageTypeSimCommand', 'kMessageTypeJoystickStatus'])
recv_count += 1
print(_FormatMessage(source, header, message, recv_count))
except socket.timeout:
pass
except BaseException as e: # pylint: disable=broad-except
sys.stdout.write('\n')
if isinstance(e, KeyboardInterrupt):
sys.exit(0)
else:
raise e
if __name__ == '__main__':
try:
argv = FLAGS(sys.argv)
except gflags.FlagsError as e:
print('%s\nUsage: %s ARGS\n%s' % (e, sys.argv[0], FLAGS))
sys.exit(1)
types = FLAGS.types
if not types:
types = message_type_helper.ShortNames()
client = aio.AioClient(
['kMessageType' + t for t in types],
timeout=0.05,
allowed_sources=['kAioNode' + t for t in FLAGS.sources])
msg_filter = _GenerateFilter(FLAGS.filter)
if FLAGS.curses:
CursesMain()
else:
ConsoleMain()
def _Sample():
aio_client = aio.AioClient(['kMessageTypeJoystickStatus'])
_PrintSample(aio_client)
def _SampleRaw():
aio_client = aio.AioClient(
['kMessageTypeJoystickRawStatus', 'kMessageTypeJoystickCommand'])
_EnableRawSamples(aio_client)
_PrintRawSample(aio_client)
_DisableRawSamples(aio_client)
def _Dump():
aio_client = aio.AioClient(['kMessageTypeJoystickStatus'])
while True:
_PrintSample(aio_client)
def __init__(self, timeout=None):
self.aio_client = aio.AioClient(
['kMessageTypeParamRequest', 'kMessageTypeParamResponse'],
timeout=timeout)
def _DumpRaw():
aio_client = aio.AioClient(
['kMessageTypeJoystickRawStatus', 'kMessageTypeJoystickCommand'])
_EnableRawSamples(aio_client)
while True:
_PrintRawSample(aio_client)
def _Calibrate():
"""Run the calibration procedure.
This functions walks the operator through each of the axes,
finding their limits and zeros.
"""
aio_client = aio.AioClient(
['kMessageTypeJoystickRawStatus', 'kMessageTypeJoystickCommand'])
axis_data = {}
_ = raw_input('Center all axes and press enter.')
for axis in AXES:
print 'Move %s axis through its limits.' % (axis)
axis_data[axis] = _CalibrateAxis(aio_client)
_ = raw_input('Move axes to their zero points and press enter.')
_EnableRawSamples(aio_client)
zero_points = _GetRawSample(aio_client)
_DisableRawSamples(aio_client)
switch_data = {}
_ = raw_input('Make sure all switches are in their down state '
'and press enter.')
for switch in SWITCHES:
print 'Move %s switch to its up state and back down.' % (switch)
switch_data[switch] = _CalibrateSwitch(aio_client)
print '# Joystick calibration parameters.'
print 'joystick_calib_params = {'
for axis in AXES:
index = axis_data[axis][0]
min_value = axis_data[axis][1]['min']
max_value = axis_data[axis][1]['max']
offset = -zero_points[index]
# TODO: Deal with axis sign calibration. Some axes are inverted
# (i.e. top is negative and bottom is positive.)
gain = 1.0 / (max_value - min_value)
if _IsPositiveAxis(axis):
axis_min = 0.0
axis_max = 1.0
else:
gain *= 2.0
axis_min = -1.0
axis_max = 1.0
cal = _GenerateAxisCalibration({
'name': axis,
'index': index,
'offset': offset,
'gain': gain,
'min': axis_min,
'max': axis_max,
})
print cal,
for switch in SWITCHES:
index = switch_data[switch][0]
min_value = switch_data[switch][1]['min']
max_value = switch_data[switch][1]['max']
cal = _GenerateSwitchCalibration({
'name': switch,
'index': index,
'min': min_value,
'max': max_value,
})
print cal,
print '}'
