def create_packet(raw, operating_mode):
"""
Override method.
Returns:
RXIPv4Packet.
Raises:
InvalidPacketException: if the bytearray length is less than 15. (start delim + length (2 bytes) + frame
type + source address (4 bytes) + dest port (2 bytes) + source port (2 bytes) + network protocol +
status + checksum = 15 bytes)
InvalidPacketException: if the length field of ``raw`` is different than its real length. (length field:
bytes 2 and 3)
InvalidPacketException: if the first byte of ``raw`` is not the header byte. See :class:`.SPECIAL_BYTE`.
InvalidPacketException: if the calculated checksum is different than the checksum field value (last byte).
InvalidPacketException: if the frame type is not :attr:`ApiFrameType.RX_IPV4`.
InvalidOperatingModeException: if ``operating_mode`` is not supported.
.. seealso::
| :meth:`.XBeePacket.create_packet`
| :meth:`.XBeeAPIPacket._check_api_packet`
"""
if operating_mode != OperatingMode.ESCAPED_API_MODE and operating_mode != OperatingMode.API_MODE:
raise InvalidOperatingModeException(operating_mode.name + " is not supported.")
XBeeAPIPacket._check_api_packet(raw, min_length=RXIPv4Packet.__MIN_PACKET_LENGTH)
if raw[3] != ApiFrameType.RX_IPV4.code:
raise InvalidPacketException("This packet is not an RXIPv4Packet.")
return RXIPv4Packet(IPv4Address(bytes(raw[4:8])), utils.bytes_to_int(raw[8:10]),
utils.bytes_to_int(raw[10:12]), IPProtocol.get(raw[12]),
raw[14:-1])
def create_packet(raw, operating_mode):
"""
Override method.
Returns:
TXIPv4Packet.
Raises:
InvalidPacketException: if the bytearray length is less than 16.
(start delim + length (2 bytes) + frame type + frame id
+ dest address (4 bytes) + dest port (2 bytes)
+ source port (2 bytes) + network protocol + transmit options
+ checksum = 16 bytes)
InvalidPacketException: if the length field of `raw` is different
from its real length. (length field: bytes 2 and 3)
InvalidPacketException: if the first byte of `raw` is not the
header byte. See :class:`.SPECIAL_BYTE`.
InvalidPacketException: if the calculated checksum is different
from the checksum field value (last byte).
InvalidPacketException: if the frame type is not
:attr:`ApiFrameType.TX_IPV4`.
InvalidOperatingModeException: if `operating_mode` is not supported.
.. seealso::
| :meth:`.XBeePacket.create_packet`
| :meth:`.XBeeAPIPacket._check_api_packet`
"""
if operating_mode not in (OperatingMode.ESCAPED_API_MODE,
OperatingMode.API_MODE):
raise InvalidOperatingModeException(op_mode=operating_mode)
XBeeAPIPacket._check_api_packet(
raw, min_length=TXIPv4Packet.__MIN_PACKET_LENGTH)
if raw[3] != ApiFrameType.TX_IPV4.code:
raise InvalidPacketException(
message="This packet is not an TXIPv4Packet.")
return TXIPv4Packet(
raw[4],
IPv4Address(bytes(raw[5:9])),
utils.bytes_to_int(raw[9:11]),
utils.bytes_to_int(raw[11:13]),
IPProtocol.get(raw[13]),
raw[14],
data=raw[15:-1]
if len(raw) > TXIPv4Packet.__MIN_PACKET_LENGTH else None,
op_mode=operating_mode)
def _parse_data(self, data):
"""
Override.
.. seealso::
| :meth:`.__ZDOCommand._parse_data`
"""
# Ensure the 16-bit address received matches the address of the device
x16 = XBee16BitAddress.from_bytes(data[1], data[0])
if x16 != self._xbee.get_16bit_addr():
return False
# Role field: 3 bits (0, 1, 2) of the next byte
role = Role.get(utils.get_int_from_byte(data[2], 0, 3))
# Complex descriptor available: next bit (3) of the same byte
complex_desc_available = utils.is_bit_enabled(data[2], 3)
# User descriptor available: next bit (4) of the same byte
user_desc_available = utils.is_bit_enabled(data[2], 4)
# Frequency band: 5 bits of the next byte
freq_band = NodeDescriptorReader.__to_bits(data[3])[-5:]
# MAC capabilities: next byte
mac_capabilities = NodeDescriptorReader.__to_bits(data[4])
# Manufacturer code: next 2 bytes
manufacturer_code = utils.bytes_to_int([data[6], data[5]])
# Maximum buffer size: next byte
max_buffer_size = int(data[7])
# Maximum incoming transfer size: next 2 bytes
max_in_transfer_size = utils.bytes_to_int([data[9], data[8]])
# Maximum outgoing transfer size: next 2 bytes
max_out_transfer_size = utils.bytes_to_int([data[13], data[12]])
# Maximum outgoing transfer size: next byte
desc_capabilities = NodeDescriptorReader.__to_bits(data[14])
self.__node_descriptor = NodeDescriptor(role, complex_desc_available, user_desc_available,
freq_band, mac_capabilities, manufacturer_code,
max_buffer_size, max_in_transfer_size,
max_out_transfer_size, desc_capabilities)
return True
def heartbeat(self, xbee: XBeeDevice, coordinator: RemoteXBeeDevice):
"""
HEARTBEAT reply/"acknowledgement"
Need to manually construct a RADIO_STATUS MAVLink message and place it at the front of
priority_queue, as RADIO_STATUS messages are automatically constructed and sent back to the
GCS on SiK radio firmware in response to a HEARTBEAT. This is crucial for establishing a
recognisable link on GCS software, such as QGroundControl.
:param xbee:
:param coordinator:
"""
logging.debug('Generating fake heartbeat')
rssi = bytes_to_int(xbee.get_parameter('DB'))
remrssi = bytes_to_int(coordinator.get_parameter('DB'))
errors = bytes_to_int(xbee.get_parameter('ER'))
radio_status_msg = self.px4.mav.radio_status_encode(
rssi=rssi, remrssi=remrssi, rxerrors=errors, txbuf=100, noise=0, remnoise=0, fixed=0)
radio_status_msg.pack(self.px4.mav)
self.queue_out.write(radio_status_msg)
def is_op_mode_valid(self, value):
"""
Returns `True` if the provided value is a valid operating mode for
the library.
Args:
value (Bytearray): The value to check.
Returns:
Boolean: `True` for a valid value, `False` otherwise.
"""
op_mode_value = utils.bytes_to_int(value)
op_mode = OperatingMode.get(op_mode_value)
if op_mode not in (OperatingMode.API_MODE,
OperatingMode.ESCAPED_API_MODE):
self._log.error(
"Operating mode '%d' (%s) not set not to loose XBee connection",
op_mode_value, op_mode.description if op_mode else "Unknown")
return False
return True
def _refresh_serial_params(self, node, parameter, value, apply=True):
"""
Refreshes the proper cached parameter depending on `parameter` value.
If `parameter` is not a cached parameter, this method does nothing.
Args:
node (:class:`.AbstractXBeeDevice`): The XBee to refresh.
parameter (String): the parameter to refresh its value.
value (Bytearray): the new value of the parameter.
apply (Boolean, optional, default=`True`): `True` to apply
immediately, `False` otherwise.
Returns:
Boolean: `True` if a network event must be sent, `False` otherwise.
"""
node_fut_apply = self._future_apply.get(str(node.get_64bit_addr()), {})
if parameter == ATStringCommand.AP.command:
new_op_mode = OperatingMode.get(utils.bytes_to_int(value))
changed = bool(new_op_mode != node.operating_mode
and new_op_mode in (OperatingMode.API_MODE,
OperatingMode.ESCAPED_API_MODE))
if changed and apply:
node._operating_mode = new_op_mode
node_fut_apply.pop(parameter, None)
elif changed:
node_fut_apply.update({parameter: value})
return changed and apply
if not node.serial_port or parameter not in (
ATStringCommand.BD.command, ATStringCommand.NB.command,
ATStringCommand.SB.command):
return False
if parameter == ATStringCommand.BD.command:
from digi.xbee.profile import FirmwareBaudrate
new_bd = utils.bytes_to_int(value)
baudrate = FirmwareBaudrate.get(new_bd)
new_bd = baudrate.baudrate if baudrate else new_bd
changed = new_bd != node.serial_port.baudrate
parameter = "baudrate" if changed and apply else parameter
value = new_bd if changed and apply else value
elif parameter == ATStringCommand.NB.command:
from digi.xbee.profile import FirmwareParity
new_parity = FirmwareParity.get(utils.bytes_to_int(value))
new_parity = new_parity.parity if new_parity else None
changed = new_parity != node.serial_port.parity
parameter = "parity" if changed and apply else parameter
value = new_parity if changed and apply else value
else:
from digi.xbee.profile import FirmwareStopbits
new_sbits = FirmwareStopbits.get(utils.bytes_to_int(value))
new_sbits = new_sbits.stop_bits if new_sbits else None
changed = new_sbits != node.serial_port.stopbits
parameter = "stopbits" if changed and apply else parameter
value = new_sbits if changed and apply else value
if changed and apply:
node.serial_port.apply_settings({parameter: value})
node_fut_apply.pop(parameter, None)
elif changed:
node_fut_apply.update({parameter: value})
return False
def _refresh_if_cached(self, node, parameter, value, apply=True):
"""
Refreshes the proper cached parameter depending on `parameter` value.
If `parameter` is not a cached parameter, this method does nothing.
Args:
node (:class:`.AbstractXBeeDevice`): The XBee to refresh.
parameter (String): the parameter to refresh its value.
value (Bytearray): the new value of the parameter.
apply (Boolean, optional, default=`True`): `True` to apply
immediately, `False` otherwise.
"""
updated = False
param = parameter.upper()
key = str(node.get_64bit_addr())
if key not in self._future_apply:
self._future_apply[key] = {}
node_fut_apply = self._future_apply.get(key, {})
# Node identifier
if param == ATStringCommand.NI.command:
node_id = str(value, encoding='utf8', errors='ignore')
changed = node.get_node_id() != node_id
updated = changed and apply
if updated:
node._node_id = node_id
node_fut_apply.pop(param, None)
elif changed:
node_fut_apply.update({param: value})
# 16-bit address / IP address
elif param == ATStringCommand.MY.command:
if XBeeProtocol.is_ip_protocol(node.get_protocol()):
ip_addr = IPv4Address(utils.bytes_to_int(value))
changed = node.get_ip_addr() != ip_addr
updated = changed and apply
if updated:
node._ip_addr = ip_addr
node_fut_apply.pop(param, None)
elif changed:
node_fut_apply.update({param: value})
elif XBee16BitAddress.is_valid(value):
x16bit_addr = XBee16BitAddress(value)
changed = node.get_16bit_addr() != x16bit_addr
updated = changed and apply
if updated:
node._16bit_addr = x16bit_addr
node_fut_apply.pop(param, None)
elif changed:
node_fut_apply.update({param: value})
elif not node.is_remote():
updated = self._refresh_serial_params(node,
param,
value,
apply=apply)
if updated:
network = node.get_local_xbee_device().get_network() if node.is_remote() \
else node.get_network()
if (network
and (not node.is_remote()
or network.get_device_by_64(node.get_64bit_addr())
or network.get_device_by_16(node.get_16bit_addr()))):
from digi.xbee.devices import NetworkEventType, NetworkEventReason
network._network_modified(NetworkEventType.UPDATE,
NetworkEventReason.READ_INFO,
node=node)
def run():
global device
#dev_logger = utils.enable_logger("digi.xbee.devices", logging.DEBUG)
dev_logger = utils.disable_logger("digi.xbee.devices")
###################################################################################################
# Look for XBee USB port, to avoid conflicts with other USB devices
###################################################################################################
rospy.init_node('fleetCoordinator', anonymous=True)
rospy.loginfo("Looking for XBee...")
context = pyudev.Context()
usbPort = 'No XBee found'
for device in context.list_devices(subsystem='tty'):
if 'ID_VENDOR' in device and device['ID_VENDOR'] == 'FTDI':
usbPort = device['DEVNAME']
device = XBeeDevice(usbPort, 57600)
#device = XBeeDevice("/dev/ttyUSB0", 57600)
device.open()
print("Current timeout: %d seconds" % device.get_sync_ops_timeout())
device.set_sync_ops_timeout(0.1)
###################################################################################################
# Get local XBee ID (should be 0, convention for Coordinator)
###################################################################################################
ID = utils.bytes_to_int(device.get_16bit_addr().address)
if ID == 0:
rospy.loginfo("\nHello,\nI am Coordinator " + str(ID)+'\n')
else:
raise Exception("This XBee device is not the coordinator of the network,\nlook for the XBee device stamped with 'C'.")
###################################################################################################
# Initialisation
###################################################################################################
#Variables storing the data received by the subscribers
global remote_devices, pubGps, pubEuler, pubLB, pubLE, lastDataStr, lastData
remote_devices = {}
lastDataStr = {}
lastData = {}
pubGps = []
pubEuler = []
pubLB = []
pubLE = []
xnet = device.get_network()
xnet.add_device_discovered_callback(network_callback)
xnet.add_discovery_process_finished_callback(network_finished)
device.add_data_received_callback(data_received)
rate = rospy.Rate(10)
GPSdata = GPSFix()
eulerAnglesData = Vector3()
lineStartData, lineEndData = Pose2D(), Pose2D()
global chosen, mode, cmd
mode = 0
chosen = 0
cmd = Twist()
pygame.init()
screen = pygame.display.set_mode( (640,480) )
pygame.display.set_caption('Python numbers')
###################################################################################################
# Transmission Loop
###################################################################################################
while not rospy.is_shutdown():
if(xnet.is_discovery_running() is False):
xnet.start_discovery_process()
display(screen)
send_command()
rate.sleep()
if(xnet.is_discovery_running()):
xnet.stop_discovery_process()
device.close()
def main(argv):
if len(argv) != 3:
print("Usage: long_test.py <port> <baud_rate> <duration_in_seconds>")
return
print(" +-------------------------------+")
print(" | Long duration and stress test |")
print(" +-------------------------------+\n")
port = argv[0]
baud_rate = int(argv[1])
duration = int(argv[2])
device = ZigBeeDevice(port, baud_rate)
try:
device.open()
# Discover the network.
network = device.get_network()
network.start_discovery_process()
print("Discovering network...")
# Wait until the discovery process has finished.
while network.is_discovery_running():
time.sleep(0.1)
if not network.has_devices():
print("No remote modules in the network")
return
# Get the first device of the network that is not an end device.
remote = None
for dev in network.get_devices():
if utils.bytes_to_int(dev.get_parameter("SM")) == 0:
remote = dev
break
if remote is None:
print("No routers in the network")
return
print("Selected remote device: %s" % remote)
# Add a data received callback.
def data_callback(message):
if message.remote_device.get_64bit_addr() == remote.get_64bit_addr(
):
print("%s - [C] - %s" %
(datetime.datetime.now(), message.data.decode()))
# Ensure that the sent and received messages are equal.
assert (data == message.data.decode())
device.add_data_received_callback(data_callback)
print("Sending data...\n")
dead_line = time.time() + duration
while dead_line > time.time():
retries = MAX_RETRIES
data_received = False
while not data_received:
try:
data = ''.join(
random.choice(string.ascii_letters)
for i in range(random.randint(1, 84)))
print("%s - [S] - %s" % (datetime.datetime.now(), data))
# Send explicit data to the loopback cluster.
device.send_expl_data(remote, data, SOURCE_ENDPOINT,
DEST_ENDPOINT, CLUSTER_ID,
PROFILE_ID)
# Read new data from the remote device.
msg = device.read_data_from(remote, 10)
print("%s - [P] - %s" %
(datetime.datetime.now(), msg.data.decode()))
data_received = True
# Ensure that the sent and received messages are equal.
assert (data == msg.data.decode())
except TimeoutException as ex:
retries -= 1
if retries == 0:
raise ex
# Wait some time between 1 and 5 seconds.
time.sleep(random.randint(1, 5))
print("\nTest finished successfully")
finally:
if device is not None and device.is_open():
device.close()
def create_packet(raw, operating_mode):
"""
Override method.
Returns:
:class:`.RouteInformationPacket`.
Raises:
InvalidPacketException: If the bytearray length is less than 46.
(start delim. + length (2 bytes) + frame type + src_event
+ length + timestamp (4 bytes) + ack timeout count
+ tx blocked count + reserved + dest addr (8 bytes)
+ src addr (8 bytes) + responder addr (8 bytes)
+ successor addr (8 bytes) + checksum = 46 bytes).
InvalidPacketException: If the length field of `raw` is different
from its real length. (length field: bytes 1 and 3)
InvalidPacketException: If the first byte of 'raw' is not the
header byte. See :class:`.SpecialByte`.
InvalidPacketException: If the calculated checksum is different
from the checksum field value (last byte).
InvalidPacketException: If the frame type is not
:attr:`.ApiFrameType.DIGIMESH_ROUTE_INFORMATION`.
InvalidPacketException: If the internal length byte of the rest
of the frame (without the checksum) is different from its real
length.
InvalidOperatingModeException: If `operating_mode` is not
supported.
.. seealso::
| :meth:`.XBeePacket.create_packet`
| :meth:`.XBeeAPIPacket._check_api_packet`
"""
if operating_mode not in (OperatingMode.ESCAPED_API_MODE,
OperatingMode.API_MODE):
raise InvalidOperatingModeException(operating_mode.name +
" is not supported.")
XBeeAPIPacket._check_api_packet(
raw, min_length=RouteInformationPacket.__MIN_PACKET_LENGTH)
if raw[3] != ApiFrameType.DIGIMESH_ROUTE_INFORMATION.code:
raise InvalidPacketException(
"This packet is not a Route Information packet.")
# 7: frame len starting from this byte (index 5) and without the checksum
if raw[5] != len(raw) - 7:
raise InvalidPacketException(
"Length does not match with the data length")
additional_data = []
if len(raw) > RouteInformationPacket.__MIN_PACKET_LENGTH:
additional_data = raw[45:]
packet = RouteInformationPacket(raw[4],
utils.bytes_to_int(raw[6:10]),
raw[10],
raw[11],
XBee64BitAddress(raw[13:21]),
XBee64BitAddress(raw[21:29]),
XBee64BitAddress(raw[29:37]),
XBee64BitAddress(raw[37:45]),
additional_data,
op_mode=operating_mode)
packet._reserved = raw[12]
return packet
def run():
global device, modeV
modeV = 0
#dev_logger = utils.enable_logger("digi.xbee.devices", logging.DEBUG)
dev_logger = utils.disable_logger("digi.xbee.devices")
###################################################################################################
# Look for XBee USB port, to avoid conflicts with other USB devices
###################################################################################################
rospy.init_node('fleetEndPoint', anonymous=True)
rospy.loginfo("Looking for XBee...")
context = pyudev.Context()
usbPort = 'No XBee found'
for device in context.list_devices(subsystem='tty'):
if 'ID_VENDOR' in device and device['ID_VENDOR'] == 'FTDI':
usbPort = device['DEVNAME']
device = XBeeDevice(usbPort, 57600)
#device = XBeeDevice("/dev/ttyUSB1", 57600)
device.open()
print("Current timeout: %d seconds" % device.get_sync_ops_timeout())
device.set_sync_ops_timeout(0.1)
###################################################################################################
# Get local XBee ID (should be 0, convention for Coordinator)
###################################################################################################
ID = utils.bytes_to_int(device.get_64bit_addr().address)
if ID == 0:
raise Exception("\nThis Shouldn't be a Coordinator" + '\n')
else:
print("Hello This is " + str(ID))
###################################################################################################
# Initialisation
###################################################################################################
#Variables storing the data received by the subscribers
global windForceString, windDirectionString, gpsString, eulerAnglesString, posString, beginString, endString, lastData, lastDataStr, pub_send_u_rudder_sail, pub_send_control_mode
windForceData, windDirectionData, GPSdata, eulerAnglesData, lineStartData, lineEndData = Float32(
), Float32(), String(), Vector3(), Pose2D(), Pose2D()
remote_devices = {}
lastDataStr = ''
lastData = {}
lastData["NMEA"] = ''
lastData["windF"] = 0.0
lastData["windT"] = 0.0
lastData["eulerx"] = 0.0
lastData["eulery"] = 0.0
lastData["eulerz"] = 0.0
#xnet = device.get_network()
#xnet.add_device_discovered_callback(network_callback)
#xnet.add_discovery_process_finished_callback(network_finished)
device.add_data_received_callback(data_received)
rate = rospy.Rate(10)
rospy.Subscriber('/sailboat/GPS/NMEA', String, sub_GPS)
rospy.Subscriber('/sailboat/wind', Pose2D, sub_WIND)
rospy.Subscriber('/sailboat/IMU', Imu, sub_IMU)
rospy.Subscriber('regulator_send_lineBegin', Pose2D, sub_lineBegin)
rospy.Subscriber('regulator_send_lineEnd', Pose2D, sub_lineEnd)
pub_send_u_rudder_sail = rospy.Publisher('xbee/sailboat_cmd',
Twist,
queue_size=2)
pub_send_control_mode = rospy.Publisher('xbee/mode', Float32, queue_size=2)
###################################################################################################
# Transmission Loop
###################################################################################################
while not rospy.is_shutdown():
#if(xnet.is_discovery_running() is False):
# xnet.start_discovery_process()
try:
device.send_data(
RemoteXBeeDevice(device,
XBee64BitAddress.from_hex_string("00")),
data_to_bytearray(lastData))
except TimeoutException:
#print("timeout")
pass
except XBeeException:
pass
if (modeV == 1):
pub_send_u_rudder_sail.publish(rudder_sail)
mode = Float32(data=0.)
mode.data = modeV
pub_send_control_mode.publish(mode)
rate.sleep()
#if(xnet.is_discovery_running()):
# xnet.stop_discovery_process()
device.close()
