import collections

import math

import numpy

import queue

import select

import socket

import struct

import threading

import time

import json

from enum import IntEnum

from typing import Sequence

from . import crc16, data, protocol

from .data import (BROADCAST_PORT, GENERAL_PORT, LAST_CHANNEL_NUMBER,

SYNC_BYTE, WAIT_TIME_FOR_RESPONSE)

from .exceptions import (ClientTimeoutError, CommunicationError, ParseError,

ResponseError)

from .protocol import ParamID, ProtocolHeader, ProtocolBottom

import logging

_logger = logging.getLogger("IXCOM")

try:

import ixcom_c_ext

ixcom_c_ext_installed = True

except ImportError:

ixcom_c_ext_installed = False

PositionTuple = collections.namedtuple('PositionTuple', 'Lon Lat Alt')

class MessageSearcherState(IntEnum):

waiting_for_sync = 0

waiting_for_msglength = 1

fetching_bytes = 2

XCOM_MAX_MESSAGE_LENGTH = 4096

XCOM_HEADER_LENGTH = ProtocolHeader().size()

XCOM_BOTTOM_LENGTH = ProtocolBottom().size()

TOTAL_MAX_MESSAGE_LENGTH = XCOM_MAX_MESSAGE_LENGTH

class MessageSearcher:

def __init__(self, parserDelegate = None, disable_crc = False, ignore_c_ext = False):

self.searcherState = MessageSearcherState.waiting_for_sync

self.currentBytes = bytearray(4096)

self.currentByteIdx = 0

self.remainingByteCount = 0

self.msgLength = 0

self.file_read_chunk_length = 2**22

self.file_write_buffer_length = 2**22

self.remaining_bytes_from_unsafe_process = bytes()

self.disableCRC = disable_crc

self.process_buffer_unsafe = self.process_buffer_unsafe_python

self.process_bytes = self.process_bytes_python

self.cfg_json = None

if ixcom_c_ext_installed and not ignore_c_ext:

self.ixcom_c_ext_obj = ixcom_c_ext.Ixcom_c_ext(valid_trigger_source_range = 6)

self.ixcom_c_ext_obj.set_crc_disable(disable_crc)

self.ixcom_c_ext_obj.set_process_bytes_callback(self.publish)

self.process_buffer_unsafe = self.ixcom_c_ext_obj.process_bytes_fast

self.process_bytes = self.ixcom_c_ext_obj.process_bytes_fast

self.callbacks = []

if parserDelegate is not None:

self.callbacks.append(parserDelegate.parse)

def handle_v5_json(self, inBytes):

if len(inBytes) < 9 * 4: # header is 8 * uint32 and crc

return 0

no_sync = inBytes[0] != SYNC_BYTE

header_len = int.from_bytes(inBytes[7 * 4:8 * 4], byteorder='little')

if len(inBytes) < 9 * 4 + header_len:

return 0

start_braket = chr(inBytes[9 * 4]) == '{'

end_braket = chr(inBytes[header_len - 1]) == '}'

if no_sync and start_braket and end_braket:

self.cfg_json = json.loads(bytes(inBytes[9*4:header_len]))

return header_len

return 0

def process_file_handle(self, file_handle):

while self.process_bytes(file_handle.read(self.file_read_chunk_length)):

pass

def process_file_handle_unsafe(self, file_handle):

while self.process_buffer_unsafe(file_handle.read(self.file_read_chunk_length)):

pass

def process_buffer_unsafe_python(self, buffer):

if len(buffer) == 0:

return 0

current_msg_start_idx = 0

last_msg_start_id = -1

inBytes = memoryview(self.remaining_bytes_from_unsafe_process+buffer)

inbytelen = len(inBytes)

if not self.remaining_bytes_from_unsafe_process:

current_msg_start_idx = self.handle_v5_json(inBytes)

while current_msg_start_idx + 5 < inbytelen:

current_msg_length = inBytes[current_msg_start_idx + 4] + 256 * inBytes[current_msg_start_idx + 5]

if current_msg_length < 20 or current_msg_length > TOTAL_MAX_MESSAGE_LENGTH:

raise Exception("File is corrupted, try xcom-remove-partial-msgs on XCOMStream file")

if current_msg_start_idx + current_msg_length > inbytelen: # Message not completely in this chunk

self.remaining_bytes_from_unsafe_process = bytes(inBytes[current_msg_start_idx:])

break

self.publish(inBytes[current_msg_start_idx:current_msg_start_idx+current_msg_length])

if current_msg_start_idx <= last_msg_start_id:

raise Exception("File is corrupted, try xcom-remove-partial-msgs on XCOMStream file")

last_msg_start_id = current_msg_start_idx

current_msg_start_idx += current_msg_length

self.remaining_bytes_from_unsafe_process = bytes(inBytes[current_msg_start_idx:])

return len(buffer)

def process_bytes_python(self, inBytes):

inByteIdx = 0

while inByteIdx < len(inBytes):

if self.searcherState == MessageSearcherState.waiting_for_sync:

poppedByte = inBytes[inByteIdx]

inByteIdx += 1

if poppedByte == SYNC_BYTE:

self.currentBytes[0] = SYNC_BYTE

self.currentByteIdx = 1

self.remainingByteCount = 5

self.searcherState = MessageSearcherState.waiting_for_msglength

elif self.searcherState == MessageSearcherState.waiting_for_msglength:

poppedByte = inBytes[inByteIdx]

inByteIdx += 1

self.currentBytes[self.currentByteIdx] = poppedByte

self.currentByteIdx += 1

self.remainingByteCount -= 1

if self.remainingByteCount == 0:

self.msgLength = self.currentBytes[self.currentByteIdx - 1] * 256 + self.currentBytes[self.currentByteIdx - 2]

self.remainingByteCount = self.msgLength - 6

if self.remainingByteCount <= (TOTAL_MAX_MESSAGE_LENGTH-6) and self.remainingByteCount >= 14: #ten more bytes from header and 4 from footer

self.searcherState = MessageSearcherState.fetching_bytes

else:

self.searcherState = MessageSearcherState.waiting_for_sync

elif self.searcherState == MessageSearcherState.fetching_bytes:

if len(inBytes) - 1 >= self.remainingByteCount + inByteIdx - 1: # Der Buffer ist Länger als der Rest der Nachricht.

self.currentBytes[self.currentByteIdx:self.currentByteIdx + self.remainingByteCount] = inBytes[inByteIdx:inByteIdx + self.remainingByteCount]

self.currentByteIdx = self.currentByteIdx + self.remainingByteCount

inByteIdx = inByteIdx + self.remainingByteCount

self.remainingByteCount = 0

else:

self.currentBytes[self.currentByteIdx:self.currentByteIdx + (len(inBytes) - inByteIdx)] = inBytes[inByteIdx:]

self.currentByteIdx = self.currentByteIdx + (len(inBytes) - inByteIdx)

self.remainingByteCount -= (len(inBytes) - inByteIdx)

inByteIdx = len(inBytes)

if self.remainingByteCount == 0:

if self.disableCRC:

self.publish(self.currentBytes[:self.currentByteIdx])

else:

crc = crc16.crc16xmodem(bytes(self.currentBytes[:self.currentByteIdx - 2]))

if crc == self.currentBytes[self.currentByteIdx - 2] + self.currentBytes[

self.currentByteIdx - 1] * 256:

self.publish(self.currentBytes[:self.currentByteIdx])

else:

pass

self.searcherState = MessageSearcherState.waiting_for_sync

return len(inBytes)

def publish(self, msg_bytes):

for callback in self.callbacks:

callback(msg_bytes)

def add_callback(self, callback):

self.callbacks.append(callback)

def remove_callback(self, callback):

self.callbacks.remove(callback)

class MessageParser:

def __init__(self):

self.subscribers = set()

self.callbacks = list()

self.messageSearcher = MessageSearcher(parserDelegate = self)

self.nothrow = False

def parse_response(self, inBytes):

message = data.ProtocolMessage()

message.header.from_bytes(inBytes[:16])

message.payload = data.ResponsePayload(message.header.msgLength)

message.from_bytes(inBytes)

self.publish(message)

def parse_parameter(self, inBytes):

parameterID = inBytes[16] + (inBytes[17] << 8)

if parameterID != ParamID.PARPLUGIN:

message = data.getParameterWithID(parameterID)

else:

pluginParameterID = inBytes[22] + (inBytes[23] << 8)

message = data.getPluginParameterWithID(pluginParameterID)

if message is not None:

try:

message.from_bytes(inBytes)

self.publish(message)

except Exception as ee:

_logger.error('Error: Parameter with ID: {} ({}) could not be parsed! Reason: {}'.format(parameterID, message.payload.get_name(), repr(ee)))

else:

data.handle_undefined_parameter(parameterID)

def parse_command(self, inBytes):

cmdID = inBytes[16] + (inBytes[17] << 8)

message = data.getCommandWithID(cmdID)

if message is not None:

message.from_bytes(inBytes)

self.publish(message)

else:

pass

def parse_plugin_message(self, inBytes):

plugin_message_id = inBytes[16] + (inBytes[17] << 8)

message = data.getPluginMessageWithID(plugin_message_id)

if message is not None:

try:

message.from_bytes(inBytes)

self.publish(message)

except ParseError as ee:

_logger.error('Error: Plugin Message with ID: {} ({}) could not be parsed! Reason: {}'.format(plugin_message_id, message.payload.get_name(), repr(ee)))

else:

data.handle_undefined_plugin_message(plugin_message_id)

def parse(self, inBytes):

header = data.ProtocolHeader()

header.from_bytes(inBytes)

try:

if header.msgID == data.MessageID.RESPONSE:

self.parse_response(inBytes)

elif header.msgID == data.MessageID.PARAMETER:

self.parse_parameter(inBytes)

elif header.msgID == data.MessageID.COMMAND:

self.parse_command(inBytes)

elif header.msgID == data.MessageID.PLUGIN:

self.parse_plugin_message(inBytes)

else:

message = data.getMessageWithID(header.msgID)

if message is not None:

message.from_bytes(inBytes)

self.publish(message)

except ParseError as err:

if self.nothrow:

_logger.error("MessageParser problem: %r", err)

else:

raise

def add_subscriber(self, subscriber):

self.subscribers.add(subscriber)

def add_callback(self, callback):

self.callbacks += [callback]

def add_callback_and_block(self, callback):

'''Add a callback function and joins the communication thread.

Call stop() in a callback function the stop the communication thread and continue.

'''

self.callbacks += [callback]

self.join_comm_thread.join()

def remove_callback(self, callback):

self.callbacks.remove(callback)

def remove_subscriber(self, subscriber):

self.subscribers.discard(subscriber)

def publish(self, message):

for subscriber in self.subscribers:

subscriber.handle_message(message, from_device=self)

for callback in self.callbacks:

callback(message, from_device=self)

class NpMessageParser(MessageParser):

def parse_response(self, inBytes):

return

def parse_parameter(self, inBytes):

parameterID = inBytes[16] + (inBytes[17] << 8)

if parameterID != ParamID.PARPLUGIN:

message = data.getStashedParameterWithID(parameterID, inBytes)

else:

pluginParameterID = inBytes[22] + (inBytes[23] << 8)

message = data.getStashedPluginParameterWithID(pluginParameterID, inBytes)

if message is not None:

try:

message.from_bytes_faster(inBytes)

self.publish(message)

except Exception as ee:

_logger.error('Error: Parameter with ID: {} ({}) could not be parsed! Reason: {}'.format(parameterID, message.payload.get_name(), repr(ee)))

else:

data.handle_undefined_parameter(parameterID)

def parse_command(self, inBytes):

cmdID = inBytes[16] + (inBytes[17] << 8)

message = data.getStashedCommandWithID(cmdID, inBytes)

if message is not None:

message.from_bytes_faster(inBytes)

self.publish(message)

else:

pass

def parse_plugin_message(self, inBytes):

plugin_message_id = inBytes[16] + (inBytes[17] << 8)

message = data.getStashedPluginMessageWithID(plugin_message_id, inBytes)

if message is not None:

try:

message.from_bytes_faster(inBytes)

self.publish(message)

except ParseError as ee:#Exception:

_logger.error('Error: Plugin Message with ID: {} ({}) could not be parsed! Reason: {}'.format(plugin_message_id, message.payload.get_name(), repr(ee)))

else:

data.handle_undefined_plugin_message(plugin_message_id)

def get_stashed_msg(self, inBytes, extra_desc = None):

msgID = inBytes[1]

if msgID >= 0xfd:

if msgID == data.MessageID.RESPONSE:

return None

elif msgID == data.MessageID.PARAMETER:

parameterID = inBytes[16] + (inBytes[17] << 8)

if parameterID != ParamID.PARPLUGIN:

message = data.getStashedParameterWithID(parameterID, inBytes, extra_desc = extra_desc)

else:

pluginParameterID = inBytes[22] + (inBytes[23] << 8)

message = data.getStashedPluginParameterWithID(pluginParameterID, inBytes, extra_desc = extra_desc)

elif msgID == data.MessageID.COMMAND:

cmdID = inBytes[16] + (inBytes[17] << 8)

message = data.getStashedCommandWithID(cmdID, inBytes, extra_desc = extra_desc)

elif msgID == data.MessageID.PLUGIN:

plugin_message_id = inBytes[16] + (inBytes[17] << 8)

message = data.getStashedPluginMessageWithID(plugin_message_id, inBytes, extra_desc = extra_desc)

else:

message = data.getStashedMessageWithID(msgID, inBytes, extra_desc = extra_desc)

if message is not None:

return message

else:

return None

def parse(self, inBytes):

msgID = inBytes[1]

if msgID >= 0xfd:

if msgID == data.MessageID.RESPONSE:

self.parse_response(inBytes)

elif msgID == data.MessageID.PARAMETER:

self.parse_parameter(inBytes)

elif msgID == data.MessageID.COMMAND:

self.parse_command(inBytes)

elif msgID == data.MessageID.PLUGIN:

self.parse_plugin_message(inBytes)

else:

message = data.getStashedMessageWithID(msgID, inBytes)

if message is not None:

message.from_bytes_faster(inBytes)

self.publish(message)

class MessageCallback:

def __init__(self, callback, msg, client):

self.callback = callback

self.msg = msg

self.client = client

def run(self):

self.callback(self.msg, self.client)

class Client(MessageParser):

'''XCOM TCP Client

Implements a TCP-socket based XCOM client and offers convenience methods to interact with the

device. Other classes may subscribe to decoded messages.

'''

def __init__(self, host, port=GENERAL_PORT, timeout = WAIT_TIME_FOR_RESPONSE):

MessageParser.__init__(self)

self.timeout = timeout

self.host = host

self.port = port

self._open_channel = -1

self._create_socket_and_connect()

self._stop_event = threading.Event()

self._response_event = threading.Event()

self._response_event.response = None

self._parameter_event = threading.Event()

self._parameter_event.parameter = None

self._message_event = threading.Event()

self._message_event.msg = None

self._message_event.id = None

self.okay_lock = threading.Lock()

self._comm_thread = threading.Thread(target = self._update_data, daemon = True, name='CommThread')

self._comm_thread.start()

self._callback_queue = queue.Queue()

self._callback_thread = threading.Thread(target = self._callback_worker, daemon = True, name='CallbackThread')

self._callback_thread.start()

self.add_subscriber(self)

def _create_socket_and_connect(self):

self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)

self.sock.connect((self.host, self.port))

def join_comm_thread(self):

'''Join the communication thread

Blocks the calling location until the communications thread terminates.

Can e.g. be used if callbacks have been set up, logs have been requested and we just want to

leave the program running like this until the communications with the device stop.

Args:

self

'''

self._comm_thread.join()

def get_commthread_handler(self):

return self._comm_thread

def get_callbackthread_handler(self):

return self._callback_thread

def get_open_channel(self):

return self._open_channel

def stop(self):

self._stop_event.set()

self._comm_thread.join()

self._callback_thread.join()

def publish(self, message):

for subscriber in self.subscribers:

subscriber.handle_message(message, from_device=self)

for callback in self.callbacks:

cb = MessageCallback(callback, message, self)

self._callback_queue.put(cb)

def __hash__(self):

'''Hash function

Implements hash to use a Client as a key in a dictionary

Args:

self

Returns:

A hash

'''

return id(self)

def __eq__(self, other):

return self is other

def _callback_worker(self):

while not self._stop_event.is_set():

try:

callback = self._callback_queue.get(block=True, timeout=1)

callback.run()

except queue.Empty:

pass

def handle_message(self, message, from_device):

if message.header.msgID == data.MessageID.RESPONSE:

self._response_event.response = message

self._response_event.set()

elif message.header.msgID == data.MessageID.PARAMETER:

self._parameter_event.parameter = message

self._parameter_event.set()

elif message.header.msgID == self._message_event.id:

self._message_event.msg = message

self._message_event.set()

def open_channel(self, channelNumber=0):

'''Opens an XCOM logical channel

Opens an XCOM logical channel on the associated socket and waits for an 'OK' response.

Args:

chnannelNumber: XCOM channel to open. Default = 0

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.XcomCommandPayload.command_id)

msgToSend.payload.data['mode'] = data.XcomCommandParameter.channel_open

msgToSend.payload.data['channelNumber'] = channelNumber

self.send_msg_and_waitfor_okay(msgToSend)

self._open_channel = channelNumber

def _update_data(self):

while not self._stop_event.is_set():

inputready, _, _ = select.select([self.sock], [],[], 0.1)

for _ in inputready:

if self.sock.fileno() != -1:

try:

_data = self.sock.recv(2048)

if len(_data) == 0:

# The socket has been closed. The above select will NOT

# wait for timeout in this case, so we would run in busy

# loop unless we wait a bit here.

time.sleep(0.1)

break # break the for-loop, test for stop event

if len(_data) >= 1024:

_logger.info('max bytes read: %s', len(_data))

self.messageSearcher.process_bytes(_data)

# self.messageSearcher.process_bytes(self.sock.recv(1024))

except OSError:

pass

def open_last_free_channel(self):

'''Opens an XCOM logical channel

Opens an XCOM logical channel on the associated socket and waits for an 'OK' response.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

RuntimeError: If no free channel is available on the system.

Returns:

channelNumber: number of the opened channel

'''

self._create_socket_and_connect()

msgToSend = data.getCommandWithID(data.XcomCommandPayload.command_id)

msgToSend.payload.data['mode'] = data.XcomCommandParameter.channel_open

channelNumber = LAST_CHANNEL_NUMBER

while channelNumber >= 0:

msgToSend.payload.data['channelNumber'] = channelNumber

try:

self.send_msg_and_waitfor_okay(msgToSend)

self.clear_all()

self._open_channel = channelNumber

return channelNumber

except (ResponseError, ConnectionError):

channelNumber -= 1

self._create_socket_and_connect()

finally:

if channelNumber == 0:

raise RuntimeError('No free channel on the system!')

def get_loglist(self, channel):

msgToSend = data.getParameterWithID(data.PARXCOM_LOGLIST2_Payload.parameter_id)

msgToSend.payload.data['reserved_paramheader'] = channel

msgToSend.payload.data['action'] = data.ParameterAction.REQUESTING

self.send_msg_and_waitfor_okay(msgToSend)

return self.wait_for_parameter()

def open_first_free_channel(self):

'''Opens an XCOM logical channel

Opens an XCOM logical channel on the associated socket and waits for an 'OK' response.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

RuntimeError: If no free channel is available on the system.

Returns:

channelNumber: number of the opened channel

'''

msgToSend = data.getCommandWithID(data.XcomCommandPayload.command_id)

msgToSend.payload.data['mode'] = data.XcomCommandParameter.channel_open

channelNumber = 0

while channelNumber <= LAST_CHANNEL_NUMBER:

msgToSend.payload.data['channelNumber'] = channelNumber

try:

self.send_msg_and_waitfor_okay(msgToSend)

self.clear_all()

self._open_channel = channelNumber

return channelNumber

except (ResponseError, ConnectionError):

channelNumber += 1

self._create_socket_and_connect()

finally:

if channelNumber > LAST_CHANNEL_NUMBER:

raise RuntimeError('No free channel on the system!')

def close_channel(self):

'''Closes XCOM logical channel

Closes XCOM channel on the associated socket and waits for an 'OK' response.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.XcomCommandPayload.command_id)

msgToSend.payload.data['mode'] = data.XcomCommandParameter.channel_close

msgToSend.payload.data['channelNumber'] = self._open_channel

self.send_msg_and_waitfor_okay(msgToSend)

self._open_channel = -1

def reboot(self):

'''Reboots the system

Sends an XCOM reboot command on the associated socket and waits for an 'OK' response.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.XcomCommandPayload.command_id)

msgToSend.payload.data['mode'] = data.XcomCommandParameter.reboot

self.send_msg_and_waitfor_okay(msgToSend)

def get_parameter(self, parameterID: int):

'''Gets parameter from device with specified ID

Gets the specified parameter. Blocks until parameter is retrieved.

Args:

parameterID: ID of the parameter to retrieve

Returns:

An XcomMessage object containing the parameter

Raises:

ClientTimeoutError: Timeout while waiting for response or parameter from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getParameterWithID(parameterID)

msgToSend.payload.data['action'] = data.ParameterAction.REQUESTING

self.send_msg_and_waitfor_okay(msgToSend)

return self.wait_for_parameter()

def get_plugin_parameter(self, parameterID: int):

'''Gets plugin parameter from device with specified ID

Gets the specified plugin parameter. Blocks until parameter is retrieved.

Args:

parameterID: ID of the plugin parameter to retrieve

Returns:

An XcomMessage object containing the parameter

Raises:

ClientTimeoutError: Timeout while waiting for response or parameter from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getPluginParameterWithID(parameterID)

msgToSend.payload.data['action'] = data.ParameterAction.REQUESTING

self.send_msg_and_waitfor_okay(msgToSend)

return self.wait_for_parameter()

def set_aligncomplete(self):

'''Completes the alignment

Completes system alignment by sending the EKF ALIGN_COMPLETE command. Blocks until system

response is received.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.ALIGN_COMPLETE

self.send_msg_and_waitfor_okay(msgToSend)

def realign(self):

'''Initiates a new alignment

Initiates a new system alignment by sending the EKF ALIGN command. Blocks until system

response is received.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.ALIGN

self.send_msg_and_waitfor_okay(msgToSend)

def save_pos(self):

'''Saves the current position

Saves the current system position in ROM. Uses the EKF SAVE_POS command

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.SAVEPOS

self.send_msg_and_waitfor_okay(msgToSend)

def save_hdg(self):

'''Saves the current heading

Saves the current heading value in ROM. Uses the EKF SAVE_HDG command

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.SAVEHDG

self.send_msg_and_waitfor_okay(msgToSend)

def forced_zupt(self, enable: bool):

'''Enables or disables forced Zero Velocity updates

Enables or disables forced Zero Velocity updates using the EKF FORCED_ZUPT command.

Args:

enable: boolean value to enable or disable ZUPTs.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.FORCED_ZUPT

msgToSend.payload.structString += 'f'

msgToSend.payload.data['enable'] = enable

self.send_msg_and_waitfor_okay(msgToSend)

def forced_zupt_param(self, enable: bool):

'''Enables or disables forced Zero Velocity updates

Enables or disables forced Zero Velocity updates using the EKF FORCED_ZUPT parameter.

Args:

enable: boolean value to enable or disable ZUPTs.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getMessageByName("PAREKF_FORCEDZUPT")

msgToSend.payload.data['action'] = data.ParameterAction.CHANGING

msgToSend.payload.data['enable'] = enable

self.send_msg_and_waitfor_okay(msgToSend)

def save_antoffset(self, antenna: int):

'''Saves the currently estimated GNSS antenna offset

Saves the currently estimated GNSS antenna offset using the EKF SAVE_ANTOFFSET command.

Args:

antenna: Antenna # to save the offset for.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_EKF_Payload.command_id)

msgToSend.payload.data['subcommand'] = data.EkfCommand.SAVEANTOFFSET

msgToSend.payload.structString += 'f'

msgToSend.payload.data['antenna'] = antenna

self.send_msg_and_waitfor_okay(msgToSend)

def save_config(self):

'''Saves the current configuration

Saves the current configuration to ROM using the CONF SAVE_CONFIG command.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_CONF_Payload.command_id)

msgToSend.payload.data['configAction'] = 0

self.send_msg_and_waitfor_okay(msgToSend)

def load_config(self):

'''Loads the configuration from ROM

Loads the configuration from ROM using the CONF LOAD_CONFIG command.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_CONF_Payload.command_id)

msgToSend.payload.data['configAction'] = 1

self.send_msg_and_waitfor_okay(msgToSend)

def load_delivery_settings(self):

'''

The current configuration will be deleted and the delivery settings will be restored after an automatic reboot

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_CONF_Payload.command_id)

msgToSend.payload.data['configAction'] = 2

self.send_msg_and_waitfor_okay(msgToSend)

def factory_reset(self):

'''Performs a factory reset

Performs a factory reset using the CONF FACTORY_RESET command.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'

'''

msgToSend = data.getCommandWithID(data.CMD_CONF_Payload.command_id)

msgToSend.payload.data['configAction'] = 4

self.send_msg_and_waitfor_okay(msgToSend)

def add_log_with_rate(self, msgID: int, rate: float):

'''Add a log with specified rate

Adds a log with a specific message ID with a specified rate. The divider is computed by

taking into account the MAINTIMING and PRESCALER system parameters.

Args:

msgID: Message ID which should be requested.

rate: Requested log rate in Hz

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK' or if rate too high.

'''

divider = self.get_divider_for_rate(rate)

self.add_log_sync(msgID, math.ceil(divider))

def get_divider_for_rate(self, rate: float):

'''Determines the divider resulting in a certain log output rate for this system.

Args:

rate: output rate in Hz

Returns:

Divider which results in an output rate as close as possible to the requested rate.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK' or if rate too high.

ValueError: If the selected rate os higher than the inertial sensor sampling rate.

'''

maintiming = self.get_parameter(data.PARSYS_MAINTIMING_Payload.parameter_id)

prescaler = self.get_parameter(data.PARSYS_PRESCALER_Payload.parameter_id)

divider = (maintiming.payload.data['maintiming'] / rate / prescaler.payload.data['prescaler'])

if divider < 1:

raise ValueError('Selected rate too high')

else:

return divider

def add_log_sync(self, msgID: int, divider: int):

'''Add a log with specified divider

Adds a log with a specific message ID with a divider.

Args:

msgID: Message ID which should be requested.

divider: divider to use

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'.

'''

msgToSend = data.getCommandWithID(data.CMD_LOG_Payload.command_id)

msgToSend.payload.data['messageID'] = msgID

msgToSend.payload.data['trigger'] = data.LogTrigger.SYNC

msgToSend.payload.data['parameter'] = data.LogCommand.ADD

msgToSend.payload.data['divider'] = divider

self.send_msg_and_waitfor_okay(msgToSend)

def add_log_event(self, msgID: int):

'''Add an event-triggered log

Adds a log with a specific message ID to be sent at event. The triggering event depends on the specific log,

e.g. for GNSSSOL, an event trigger will trigger the log with every new solution.

Args:

msgID: Message ID which should be requested.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'.

'''

msgToSend = data.getCommandWithID(data.CMD_LOG_Payload.command_id)

msgToSend.payload.data['messageID'] = msgID

msgToSend.payload.data['trigger'] = data.LogTrigger.EVENT

msgToSend.payload.data['parameter'] = data.LogCommand.ADD

msgToSend.payload.data['divider'] = 500 # use 500 here, because a '1' is rejected from some logs

self.send_msg_and_waitfor_okay(msgToSend)

def clear_all(self):

'''Clears all logs

Sends a CLEAR_ALL command to the system.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'.

'''

msgToSend = data.getCommandWithID(data.CMD_LOG_Payload.command_id)

msgToSend.payload.data['messageID'] = 3

msgToSend.payload.data['trigger'] = data.LogTrigger.SYNC

msgToSend.payload.data['parameter'] = data.LogCommand.CLEAR_ALL

msgToSend.payload.data['divider'] = 1

self.send_msg_and_waitfor_okay(msgToSend)

def clear_log(self, msgID: int):

'''Clears a log with a specific message ID.

Clears a log with a specific message ID.

Args:

msgID: Message ID which should be requested.

Raises:

ClientTimeoutError: Timeout while waiting for response from the XCOM server

ResponseError: The response from the system was not 'OK'.

'''

msgToSend = data.getCommandWithID(data.CMD_LOG_Payload.command_id)

msgToSend.payload.data['messageID'] = msgID

msgToSend.payload.data['trigger'] = data.LogTrigger.SYNC

msgToSend.payload.data['parameter'] = data.LogCommand.CLEAR

msgToSend.payload.data['divider'] = 1

self.send_msg_and_waitfor_okay(msgToSend)

def poll_log(self, msgID):

'''Polls a log

Polls a log with a specified message ID. Blocks until the log is retrieved and returns

the log.

Args:

msgID: Message ID to poll

Returns:

The polled log

Raises:

ClientTimeoutError: Timeout while waiting for response or log from the XCOM server

ResponseError: The response from the system was not 'OK'.

'''

msgToSend = data.getCommandWithID(data.CMD_LOG_Payload.command_id)

msgToSend.payload.data['messageID'] = msgID

msgToSend.payload.data['trigger'] = data.LogTrigger.POLLED

msgToSend.payload.data['parameter'] = data.LogCommand.ADD

msgToSend.payload.data['divider'] = 500 # use 500 here, because a '1' is rejected from some logs

self._message_event.id = msgID

self._message_event.clear()

self.send_msg_and_waitfor_okay(msgToSend)

return self.wait_for_polled_log()

def wait_for_parameter(self):

'''Waits for reception of parameter

Blocks until a parameterEvent is received.

Raises:

ClientTimeoutError: Timeout while waiting for parameter from the XCOM server

'''

self._update_until_event(self._parameter_event, self.timeout)

result = self._parameter_event.parameter

return result

def _update_until_event(self, event, timeout):

event.wait(timeout=timeout)

if event.is_set():

event.clear()

return

raise ClientTimeoutError('Timeout while waiting for event', thrower=self)