HelperA2GMeasurements

Communication between the nodes handler

Bases: object

Python class for handling the interaction between the multiple devices available in the system: gps, gimbal and rfsoc.

It creates instances of each class handling one of those devices (i.e. GimbalRS2, GpsSignaling).

It creates a wireless TCP connection between the host computers of both nodes (air and ground node) to control devices and retrieve information from them.

It creates a thread (called here communication thread) to handle the communication between the host computers of both nodes.

• MAX_TIME_EMPTY_SOCKETS: the maximum allowed time to wait if no information was sent over a socket.

• CONN_MUST_OVER_FLAG: a (boolean) flag indicating whether to close or not the connection between the host computers.

• drone_fm_flag: a (boolean) flag set at the GUI indicating whether the air node's gimbal should follow the ground node. In all our documentation, a gimbal is said to be in "follow mode" (don't confuse with DJI's RS2 camera-based follow mode) if it follows the other node.

• PAP_TO_PLOT: a numpy array (of size defined in the method pipeline_operations_rfsoc_rx_ndarrayof the class RFSoCRemoteControlFromHost) used to plot the Power Angular Profile (PAP) of the wireless channel in GUI's PAP Panel.

Source code in a2gmeasurements.py

class HelperA2GMeasurements(object):
    """
    Python class for handling the interaction between the multiple devices available in the system: gps, gimbal and rfsoc.

    It creates instances of each class handling one of those devices (i.e. GimbalRS2, GpsSignaling).

    It creates a wireless TCP connection between the host computers of both nodes (air and ground node) to control devices and retrieve information from them.

    It creates a thread (called here communication thread) to handle the communication between the host computers of both nodes.

    * ``MAX_TIME_EMPTY_SOCKETS``: the maximum allowed time to wait if no information was sent over a socket.

    * ``CONN_MUST_OVER_FLAG``: a (boolean) flag indicating whether to close or not the connection between the host computers.

    * ``drone_fm_flag``: a (boolean) flag set at the GUI indicating whether the air node's gimbal should follow the ground node. In all our documentation, a gimbal is said to be in "follow mode" (don't confuse with DJI's RS2 camera-based follow mode) if it follows the other node. 

    * ``PAP_TO_PLOT``: a numpy array (of size defined in the method ``pipeline_operations_rfsoc_rx_ndarray``of the class ``RFSoCRemoteControlFromHost``) used to plot the Power Angular Profile (PAP) of the wireless channel in GUI's PAP Panel.
    """
    def __init__(self, ID, SERVER_ADDRESS, 
                 DBG_LVL_0=False, DBG_LVL_1=False, 
                 IsGimbal=False, IsGPS=False, IsSignalGenerator=False, IsRFSoC=False,
                 rfsoc_static_ip_address=None, #uses the default ip_adress
                 F0=None, L0=None,
                 SPEED=0,
                 GPS_Stream_Interval='msec500', AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN=0.001,
                 operating_freq=57.51e9,
                 heading_offset=0):
        """
        Creates instances of classes ``GimbalRS2`` (or ``GimbalGremsyH16``), ``GpsSignaling``, ``RFSoCRemoteControlFromHost`` to control these devices.

        Args:
            ID (str): either 'DRONE' or 'GND'.
            SERVER_ADDRESS (str): the IP address of the ground station.
            DBG_LVL_0 (bool, optional): if set, prints some low-level messages usefull for debugging. Defaults to False.
            DBG_LVL_1 (bool, optional): if set, prints some higher-level messages usefull for debugging. Defaults to False.
            IsGimbal (bool, optional): 0 or FALSE, when no gimbal is physically connected to this host computer; 1, when a Ronin RS2 is physically connected; 2, when a Gremsy H16 is physically connected. Defaults to False.
            IsGPS (bool, optional): True if a gps is physically connected to this host computer. False otherwise. Defaults to False.
            IsSignalGenerator (bool, optional): True if a signal generator controlled by pyvisa commands is physically connected to this host computer. False otherwise. Defaults to False.
            IsRFSoC (bool, optional): True if an RFSoC is physically connected to this host computer. False otherwise. Defaults to False.
            rfsoc_static_ip_address (str, optional): IP address of the RFSoC connected to this host computer. Defaults to None.
            L0 (float, optional): parameter of the signal generator. Defaults to None.
            SPEED (int, optional): the speed of the node in m/s. If this node is GROUND it should be 0 (gnd node does not move) as it is by default. This parameter ONLY incides in raising a warning debug print when the speed of the node is higher than the time difference between consecutive SBF sentences. NOT a crutial parameter at all. Stays here for back compatibility. Defaults to 0.
            GPS_Stream_Interval (str, optional): time interval used for the retrieving of the configured SBF sentences in Septentrio's receiver connected to this host computer. A list of available options is shown in ``start_gps_data_retrieval`` of class ``GpsSignaling``. Defaults to 'msec500'.
            AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN (float, optional): approximated time between calls of the communication thread. This parameter is used in conjunction with ``MAX_TIME_EMPTY_SOCKETS`` to raise an exception when neither side of the communication link is sending any message. Unfortunately, this is a very simple estimate, since the actual time between calls depends on many factors and is does not remain constant between calls. Defaults to 0.001.
            operating_freq (_type_, optional): operating frequency of the Sivers RF-frontend. The range of defined frequencies is defined in the "User Manual EVK06002" of the Sivers EVK (57-71 GHz). Defaults to 57.51e9.
            heading_offset (int, optional): heading offset (check its definition in the ``GpsSignaling.setHeadingOffset`` method). Defaults to 0.
        """

        self.AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN = AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN
        self.MAX_TIME_EMPTY_SOCKETS = 20 # in [s]
        self.MAX_NUM_RX_EMPTY_SOCKETS = round(self.MAX_TIME_EMPTY_SOCKETS / self.AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN)
        self.rxEmptySockCounter = 0

        self.ID = ID
        self.SERVER_ADDRESS = SERVER_ADDRESS  
        self.SOCKET_BUFFER = []
        self.DBG_LVL_0 = DBG_LVL_0
        self.DBG_LVL_1 = DBG_LVL_1
        self.IsGimbal = IsGimbal
        self.IsGPS = IsGPS
        self.IsRFSoC = IsRFSoC
        self.IsSignalGenerator = IsSignalGenerator
        self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD = -7.5e3 
        self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM = -8.5e3
        self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY = -9.5e3
        self.SPEED_NODE = SPEED # m/s
        self.CONN_MUST_OVER_FLAG = False # Usefull for drone side, as its script will poll for looking if this is True
        self.PAP_TO_PLOT = []
        self.drone_fm_flag = False

        print(IsGPS, self.IsGPS)

        if IsRFSoC:
            self.myrfsoc = RFSoCRemoteControlFromHost(operating_freq=operating_freq, rfsoc_static_ip_address=rfsoc_static_ip_address)
            print("[DEBUG]: Created RFSoC class")
        if IsGimbal == 1: # By default, the TRUE value is GimbalRS2
            self.myGimbal = GimbalRS2()
            self.myGimbal.start_thread_gimbal()
            time.sleep(0.5)
            print("[DEBUG]: Created Gimbal class")
        elif IsGimbal == 2:
            self.myGimbal = GimbalGremsyH16()
            self.myGimbal.start_thread_gimbal()
            print("[DEBUG]: Created Gimbal class")
        else: # IsGimbal = False
            print("[DEBUG]: No gimbal class is created")
        if IsGPS:
            self.mySeptentrioGPS = GpsSignaling(DBG_LVL_2=True, DBG_LVL_1=False, DBG_LVL_0=False)
            print("[DEBUG]: Created GPS class")
            self.mySeptentrioGPS.serial_connect()

            if self.mySeptentrioGPS.GPS_CONN_SUCCESS:
                self.mySeptentrioGPS.serial_instance.reset_input_buffer()

                # Set the heading offset if any
                self.mySeptentrioGPS.setHeadingOffset(heading_offset)

                if self.ID == 'DRONE':
                    self.mySeptentrioGPS.start_gps_data_retrieval(stream_number=1,  msg_type='SBF', interval=GPS_Stream_Interval, sbf_type='+PVTCartesian+AttEuler')
                elif self.ID == 'GROUND':
                    self.mySeptentrioGPS.start_gps_data_retrieval(stream_number=1,  msg_type='SBF', interval=GPS_Stream_Interval, sbf_type='+PVTCartesian+AttEuler')
                print("[DEBUG]: started gps stream")
                #self.mySeptentrioGPS.start_gps_data_retrieval(msg_type='NMEA', nmea_type='GGA', interval='sec1')
                self.mySeptentrioGPS.start_thread_gps()
                time.sleep(0.5)
        if IsSignalGenerator:
            rm = pyvisa.ResourceManager()
            inst = rm.open_resource('GPIB0::19::INSTR')
            self.inst = inst
            self.inst.write('F0 ' + str(F0) + ' GH\n')
            self.inst.write('L0 ' + str(L0)+ ' DM\n')
            time.sleep(0.5)

    def gimbal_follows_drone(self, heading=None, lat_ground=None, lon_ground=None, height_ground=None, 
                                    lat_drone=None, lon_drone=None, height_drone=None, fmode=0x00):
        """
        Computes the yaw, pitch and roll angles required to move the gimbal in this node towards the other node.

        The caller of this function must guarantee that if ``self.ID == 'GROUND'``, the arguments passed to this function are drone coords. The ground coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

        The caller of this function must guarantee that if ``self.ID == 'DRONE'``, the arguments passed to this function are ground coords. The drone coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

        If ``IsGPS`` is False (no GPS connected), then ``heading``, ``lat_ground``, ``lon_ground``, ``height_ground``, ``lat_drone``, ``lon_drone``, ``height_drone`` must be provided. 

        In that case, all coordinates provided must be geodetic (lat, lon, alt).

        Args:
            heading (float, optional): angle between [0, 2*pi] (rads) corresponding to the heading of the line between the two antennas connected to Septentrio's receiver in this node. Defaults to None.
            lat_ground (float, optional): latitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.
            lon_ground (float, optional): longitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.
            height_ground (float, optional): height of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.
            lat_drone (float, optional): latitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.
            lon_drone (float, optional): longitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.
            height_drone (float, optional): height of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.
            fmode (hexadecimal, optional): defines if the gimbal will follow the other node in Azimuth, elevation or both of them. Options are: 0x00, for Azimuth and elevation; 0x01, for Elevation, 0x02, for Azimuth. Defaults to 0x00.

        Returns:
            yaw_to_set (int, optional): yaw angle to be set at the gimbal of this node, in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458)
            pitch_to_set(int, optional): pitch angle to be set at the gimbal of this node, to follow the other node. The actual value is the angle value in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458).
        """

        if self.IsGPS:
            if fmode == 0x00 or fmode == 0x02:
                coords, head_info = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Both')

                if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                    return self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL
                elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ:
                    return self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ
                else:
                    heading = head_info['Heading']
                    time_tag_heading = head_info['TOW']
                    time_tag_coords = coords['TOW']
                    datum_coordinates = coords['Datum']
            elif fmode == 0x01:
                coords = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

                if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                    return self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL
                elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ:
                    return self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ
                else:
                    time_tag_coords = coords['TOW']
                    datum_coordinates = coords['Datum']

            '''
            Check if the time difference (ms) between the heading and the coordinates info is less
            than the time it takes the node to move a predefined distance with the actual speed.           
            If the node is not moving (self.SPEED = 0) it means the heading info will be always the same
            and the check is not required.
            '''
            if self.SPEED_NODE > 0:
                time_distance_allowed = 2 # meters
                if  abs(time_tag_coords - time_tag_heading) > (time_distance_allowed/self.SPEED_NODE)*1000:
                    print('\n[WARNING]: for the time_distance_allowed the heading info of the grounde node does not correspond to the coordinates')
                    return self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD, self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD, self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD

            if self.ID == 'GROUND':
            # Convert Geocentric WGS84 to Geodetic to compute distance and Inverse Transform Forward Azimuth (ITFA) 
                if datum_coordinates == 0:
                    lat_ground, lon_ground, height_ground = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'])
                # Geocentric ETRS89
                elif datum_coordinates == 30:
                    lat_ground, lon_ground, height_ground = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'], EPSG_GEOCENTRIC=4346)
                else:
                    print('\n[ERROR]: Not known geocentric datum')
                    return self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM

            elif self.ID == 'DRONE':
                # Convert Geocentric WGS84 to Geodetic to compute distance and Inverse Transform Forward Azimuth (ITFA) 
                if datum_coordinates == 0:
                    lat_drone, lon_drone, height_drone = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'])
                # Geocentric ETRS89
                elif datum_coordinates == 30:
                    lat_drone, lon_drone, height_drone = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'], EPSG_GEOCENTRIC=4346)
                else:
                    print('\n[ERROR]: Not known geocentric datum')
                    return self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM
        # Testing mode
        elif self.IsGPS == False:
            # Both coordinates must be provided and must be in geodetic format
            1

        if (lat_ground is None and lat_drone is None) or (lon_ground is None and lon_drone is None) or (height_ground is None and height_drone is None):
            print("\n[ERROR]: Either ground or drone coordinates MUST be provided")
            return self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY, self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY, self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY       

        if fmode == 0x00: # Azimuth and elevation
            if self.ID == 'DRONE':
                yaw_to_set = azimuth_difference_between_coordinates(heading, lat_drone, lon_drone, lat_ground, lon_ground)
                pitch_to_set = elevation_difference_between_coordinates(lat_drone, lon_drone, height_drone, lat_ground, lon_ground, height_ground)
            elif self.ID == 'GROUND':
                yaw_to_set = azimuth_difference_between_coordinates(heading, lat_ground, lon_ground, lat_drone, lon_drone)
                pitch_to_set = elevation_difference_between_coordinates(lat_ground, lon_ground, height_ground, lat_drone, lon_drone, height_drone)
        elif fmode == 0x02: # Azimuth
            if self.ID == 'DRONE':
                yaw_to_set = azimuth_difference_between_coordinates(heading, lat_drone, lon_drone, lat_ground, lon_ground)
            elif self.ID == 'GROUND':
                yaw_to_set = azimuth_difference_between_coordinates(heading, lat_ground, lon_ground, lat_drone, lon_drone)
        elif fmode == 0x01: # Elevation
            if self.ID == 'DRONE':
                pitch_to_set = elevation_difference_between_coordinates(lat_drone, lon_drone, height_drone, lat_ground, lon_ground, height_ground)
            elif self.ID == 'GROUND':
                pitch_to_set = elevation_difference_between_coordinates(lat_ground, lon_ground, height_ground, lat_drone, lon_drone, height_drone)

        return yaw_to_set, pitch_to_set

    def do_follow_mode_gimbal(self, fmode=0x00):
        """
        Callback function when this node receives a ``FOLLOWGIMBAL`` command.

        The ``FOLLOWGIMBAL`` command is sent when the other node asks for this node's GPS information to be able to follow this node's movement.        

        Args:
            fmode (hexadecimal, optional): specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.
        """

        self.do_getgps_action(follow_mode_gimbal=True, fmode=0x00)

    def do_getgps_action(self, follow_mode_gimbal=False, fmode=0x00):
        """
        Callback function when this node receives a ``GETGPS`` command.

        The ``GETGPS`` commmand differentiates from ``FOLLOWGIMBAL`` in that when the other node only request GPS information from this node (i.e. for display the coordinates on a panel of the GUI), the ``follow_mode_gimbal`` is False as well as the ``FMODE`` key of the sent dictionary.

        Args:
            follow_mode_gimbal (bool, optional): True if other node's gimbal must follow this node's movement. Defaults to False.
            fmode (hexadecimal, optional): specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.
        """

        if self.DBG_LVL_1:
            print(f"THIS ({self.ID}) receives a GETGPS command")

        if self.IsGPS:            
            # Only need to send to the OTHER station our last coordinates, NOT heading.
            # Heading info required by the OTHER station is Heading info from the OTHER station

            # It has to send over the socket the geocentric/geodetic coordinates
            # The only way there are no coordinates available is because:
            # 1) Didn't start gps thread with PVTCart and AttEuler type of messages
            # 2) Messages interval is too long and the program executed first than the first message arrived
            # 3) The receiver is not connected to enough satellites or multipath propagation is very strong, so that ERROR == 1

            data_to_send = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

            if data_to_send['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                # More verbose
                print(f"[WARNING]: This {self.ID} has nothing on GPS buffer")
                return

            elif data_to_send['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
                # More verbose
                print(f"[WARNING]: This {self.ID} does not have GPS or GPS signals are not available")
                return

            if follow_mode_gimbal:
                print('[DEBUG]: Last coordinates retrieved and followgimbal flag set to True to be sent')
                data_to_send['FOLLOW_GIMBAL'] = 0x01
                data_to_send['FMODE'] = fmode

            # data_to_send wont be any of the other error codes, because they are not set for 'what'=='Coordinates'
            else:            
                data_to_send['FOLLOW_GIMBAL'] = 0x02

            if self.ID == 'GROUND':
                frame_to_send = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x03, cmd=0x01, data=data_to_send)
            elif self.ID == 'DRONE':
                frame_to_send = self.encode_message(source_id=0x02, destination_id=0x01, message_type=0x03, cmd=0x01, data=data_to_send)

            if self.DBG_LVL_1:
                print('\n[DEBUG_1]:Received the GETGPS and read the SBF buffer')
            if self.ID == 'GROUND':
                self.a2g_conn.sendall(frame_to_send)
            if self.ID == 'DRONE':
                self.socket.sendall(frame_to_send)

            if self.DBG_LVL_1:
                print('\n[DEBUG_1]: Sent SBF buffer')

        else:
            #print('[WARNING]:ASKED for GPS position but no GPS connected: IsGPS is False')
            1

    def do_setgimbal_action(self, msg_data):
        """
        Callback function when this node receives a ``SETGIMBAL`` command.

        Args:
            msg_data (dict): dictionary with keys 'YAW', 'PITCH' and 'MODE'. The 'YAW' values range between [-1800, 1800]. The 'PITCH' values are restricted (by software) to the interval [-600, 600] to avoid hits between the case and the gimbal. The 'MODE' values are: 0x00, consider 'YAW' and/or 'PITCH' values as relative to the actual position of the gimbal; 0x01, consider 'YAW' and/or 'PITCH' values as relative to the absolute 0 (in both azimuth and elevation) position.
        """

        if self.IsGimbal!=0:
            # Unwrap the dictionary containing the yaw and pitch values to be set.
            #msg_data = json.loads(msg_data)

            # Error checking
            #if 'YAW' not in msg_data or 'PITCH' not in msg_data or 'MODE' not in msg_data:
            if 'YAW' not in msg_data or 'PITCH' not in msg_data:
                if 'MODE' not in msg_data:
                    print('[ERROR]: no YAW or PITCH provided')
                    return
                else:
                    self.myGimbal.change_gimbal_mode(mode=msg_data['MODE'])                    
            elif 'YAW' in msg_data and 'PITCH' in msg_data:
                if float(msg_data['YAW']) > 1800 or float(msg_data['PITCH']) > 600 or float(msg_data['YAW']) < -1800 or float(msg_data['PITCH']) < -600:
                    print('[ERROR]: Yaw or pitch angles are outside of range')
                    return
                else:
                    if self.IsGimbal == 1: # RS2
                        # Cast to int values as a double check, but values are send as numbers and not as strings.
                        self.myGimbal.setPosControl(yaw=int(msg_data['YAW']), roll=0, pitch=int(msg_data['PITCH']), ctrl_byte=msg_data['MODE'])
                    if self.IsGimbal == 2: # Gremsy
                        # Cast to int values as a double check, but values are send as numbers and not as strings.
                        self.myGimbal.setPosControl(yaw=float(msg_data['YAW']), pitch=float(msg_data['PITCH']), mode=msg_data['MODE'])
        else:
            print('\n[WARNING]: Action to SET Gimbal not posible cause there is no gimbal: IsGimbal is False')

    def do_start_meas_drone_rfsoc(self, msg_data):
        """
        Callback function when this node receives a ``STARTDRONERFSOC`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        The purpose is to start the RFSoC thread (created in ``RFSoCRemoteControlFromHost`` class) responsible for retrieving the measured Channel Impulse Response from the RFSoC.

        It is assumed that prior to this callback, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.

        Args:
            msg_data (dict): dictionary with keys 'carrier_freq', 'rx_gain_ctrl_bb1', 'rx_gain_ctrl_bb2', 'rx_gain_ctrl_bb3', 'rx_gain_ctrl_bfrf'. More information about these keys can be found in method ``set_rx_rf`` from ``RFSoCRemoteControlFromHost`` class.
        """

        if self.ID == 'DRONE': # double check that we are in the drone
            print("[DEBUG]: Received REQUEST to START measurement")
            self.myrfsoc.start_thread_receive_meas_data(msg_data)
            self.STOP_SEND_SETIRF_FLAG = False

    def do_stop_meas_drone_rfsoc(self):
        """
        Callback function when this node receives a ``STOPDRONERFSOC`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        The purpose is to stop the RFSoC thread.

        It is assumed that prior to this function, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.
        """
        if self.ID == 'DRONE': # double check that we are in the drone
            print("[DEBUG]: Received REQUEST to STOP measurement")
            self.myrfsoc.stop_thread_receive_meas_data()
            self.STOP_SEND_SETIRF_FLAG = True

    def do_finish_meas_drone_rfsoc(self):
        """
        Callback function when this node receives a ``FINISHDRONERFSOC`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        The purpose is to finish the experiment (as defined in "Manual A2GMeasurements"). When the experiment is finished the GUI allows the user to end (disconnect) the connection between both nodes.
        """
        if self.ID == 'DRONE': # double check that we are in the drone
            print("[DEBUG]: Received REQUEST to FINISH measurement")
            self.myrfsoc.finish_measurement()
            self.STOP_SEND_SETIRF_FLAG = True

    def do_set_irf_action(self, msg_data):
        """
        Callback function when this node receives a ``SETIRF`` command.

        This comand is unidirectional. It is always sent by the drone node to the ground node.

        Receives from the drone a subsampled version of the Power Angular Profile for it to be used by the GUI to continuously plot it in its PAP panel.

        Args:
            msg_data (numpy.ndarray): attribute value ``data_to_visualize`` from ``RFSoCRemoteControlFromHost`` class.
        """

        if self.ID == 'GROUND': # double checj that we are in the gnd
            self.PAP_TO_PLOT = np.asarray(msg_data)

    def do_closed_gui_action(self):
        """
        Callback function when this node receives a ``CLOSEDGUI`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        Sets a flag indicating (the drone node) that it can end its main script, since the GUI was closed by the user at the ground node.
        """

        if self.ID == 'DRONE':
            self.CONN_MUST_OVER_FLAG = True

    def do_set_remote_fm_flag(self, data=None):
        """
        Callback function when this node receives a ``SETREMOTEFMFLAG`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        Sets the ``drone_fm_flag``. When this flag is set, the drone node can start sending ``FOLLOWGIMBALL`` commands to the ground node to get ground node's coordinates and be able to follow (drone node) it (ground node).

        Args:
            data (dict, optional): dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'MOBILITY', corresponding to geocentric coordinates. The mentioned keys and their corresponding values refer to the ground node. The coordinates will be available if the ground node ``MOBILITY`` is ``static`` (0x01). The ``FMODE`` key refers to the wheter the drone gimbal follows the ground node in azimuth (0x02), elevation (0x01) or both (0x00) .Defaults to None.
        """

        if self.ID == 'DRONE':
            self.drone_fm_flag = True
            self.remote_config_for_drone_fm = data

    def do_set_remote_stop_fm(self):
        """
        Callback function when this node receives a ``SETREMOTESTOPFM`` command.

        This comand is unidirectional. It is always sent by the ground node to the drone node.

        Unsets the ``drone_fm_flag`` flag.
        """

        if self.ID == 'DRONE':
            self.drone_fm_flag = False

    def process_answer_get_gps(self, data):
        """
        Callback function when this node receives an ``ANS`` type of message (the equivalent to an acknowledment) from the other node, after this node sent to the other node a ``GETGPS`` or a ``FOLLOWGIMBAL`` command.

        Args:
            data (dict): dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'FOLLOW_GIMBAL'. The values of 'X', 'Y', 'Z' are the geocentric coordinates from the other node. 'FMODE' is either 0x00 (Elevation and Azimuth), 0x01 (Elevation) or 0x02 (Azimuth). 'FOLLOW_GIMBAL' is either True (when the sent command by this node was ``FOLLOWGIMBAL``) or False (when the sent command by this node was ``GETGPS``)
        """

        if self.DBG_LVL_1:
            print(f"THIS ({self.ID}) receives protocol ANS")

        if self.ID =='DRONE':
            y_gnd = data['Y']
            x_gnd = data['X']

            datum_coordinates = data['Datum']

            if y_gnd == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL or x_gnd == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
                print('[ERROR]: no GPS coordinates received from DRONE through socket link')
                return

                # THE HEIGHT VALUE IS THE ALTITUDE VALUE OVER THE SEA LEVEL
                # Z is in geocentric coordinates and does not correspond to the actual height:
                # Geocentric WGS84
            if datum_coordinates == 0:
                lat_gnd, lon_gnd, height_gnd = geocentric2geodetic(x_gnd, y_gnd, data['Z'])
                # Geocentric ETRS89
            elif datum_coordinates == 30:
                lat_gnd, lon_gnd, height_gnd = geocentric2geodetic(x_gnd, y_gnd, data['Z'], EPSG_GEOCENTRIC=4346)
            else:
                print('[ERROR]: Not known geocentric datum')
                return

            yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_ground=lat_gnd, lon_ground=lon_gnd, height_ground=height_gnd, fmode=data['FMODE'])

            while ((yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ) or (pitch_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ)):
                yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_ground=lat_gnd, lon_ground=lon_gnd, height_ground=height_gnd, fmode=data['FMODE'])
        elif self.ID == 'GROUND':
            y_drone = data['Y']
            x_drone = data['X']

            datum_coordinates = data['Datum']

            if y_drone == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL or x_drone == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
                print('[ERROR]: no GPS coordinates received from DRONE through socket link')
                return

                # Z is in geocentric coordinates and does not correspond to the actual height:
                # Geocentric WGS84
            if datum_coordinates == 0:
                lat_drone, lon_drone, height_drone = geocentric2geodetic(x_drone, y_drone, data['Z'])
                # Geocentric ETRS89
            elif datum_coordinates == 30:
                lat_drone, lon_drone, height_drone = geocentric2geodetic(x_drone, y_drone, data['Z'], EPSG_GEOCENTRIC=4346)
            else:
                print('[ERROR]: Not known geocentric datum')
                return

            # This is for GUI GPS panel to show drone coordinates
            self.last_drone_coords_requested = {'LAT': lat_drone, 'LON': lon_drone}

            yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_drone=lat_drone, lon_drone=lon_drone, height_drone=height_drone, fmode=data['FMODE'])

                # If error [yaw, pitch] values because not enough gps buffer entries (but gps already has entries, meaning is working), call again the gimbal_follows_drone method
            while ((yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ) or (pitch_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ)):
                yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_drone=lat_drone, lon_drone=lon_drone, height_drone=height_drone, fmode=data['FMODE'])

        if yaw_to_set == self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD or yaw_to_set == self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM or yaw_to_set == self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY or yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
            print('[ERROR]: one of the error codes of gimbal_follows_drone persists')
            print(f"[DEBUG]: This {self.ID} gimbal will NOT follow its pair node due to ERROR")
        else:
            print(f"[DEBUG]: This {self.ID} YAW to set is: {yaw_to_set}, and PITCH to set is: {pitch_to_set}")

            if data['FOLLOW_GIMBAL'] == 0x01: # True, Follow gimbal
                if self.IsGimbal!=0: # There is a gimbal at the node that receives the answer to its command request.
                    self.myGimbal.setPosControl(yaw=yaw_to_set, pitch=pitch_to_set) 
                    print(f"[DEBUG]: This {self.ID} gimbal WILL follow its pair node as stated by user")
                else:
                    print('[WARNING]: No gimbal available, so no rotation will happen')
            elif data['FOLLOW_GIMBAL'] == 0x02: # False
                print(f"[DEBUG]: This {self.ID} gimbal will NOT follow its pair node as stated by user")

    def decode_message(self, data):
        """
        Parses an incoming TCP message and calls the appropriate function to handle it. 

        This function is called by ``socket_receive`` (the communication thread callback).

        Args:
            data (bytes): raw data to be decoded
        """

        source_id, destination_id, message_type, cmd, length = struct.unpack('BBBBB', data[:5])
        data_bytes = data[5:]

        if message_type == 0x01: # SHORT cmd type message
            if cmd == 0x01 and length == 1: # FOLLOWGIMBAL
                print(f"[DEBUG]: THIS {self.ID} receives FOLLOWGIMBAL cmd")
                data_bytes = data_bytes[:1]
                self.do_follow_mode_gimbal(fmode=data_bytes)
            elif cmd == 0x02 and length == 0: # GETGPS
                print(f"[DEBUG]: THIS {self.ID} receives GETGPS cmd")
                self.do_getgps_action()
            elif cmd == 0x03 and length == 4: # SETGIMBAL
                print(f"[DEBUG]: THIS {self.ID} receives SETGIMBAL cmd")
                data_bytes = data_bytes[:13] # 3 float32 array entries + 1 byte
                yaw, pitch, roll, mode = struct.unpack('fffB', data_bytes)
                self.do_setgimbal_action({'YAW': yaw, 'ROLL': roll, 'PITCH': pitch, 'MODE': mode})
            elif cmd == 0x04 and length == 5: # STARTDRONERFSOC
                print(f"[DEBUG]: THIS {self.ID} receives STARTDRONERFSOC cmd")
                data_bytes = data_bytes[:12] # 1 float32 and 4 int16
                carr_freq, rx_1, rx_2, rx_3, rx_bfrf = struct.unpack('fHHHH', data_bytes)

                # float round-error check
                if carr_freq > 70e9 and np.abs(carr_freq-70e9) < 1500: # float round-error of 1.5 kHz
                    carr_freq = 70e9
                elif carr_freq < 57.51e9 and np.abs(carr_freq-57.51e9) < 1500: #float round-error of 1.5 kHz
                    carr_freq = 57.51e9
                msg_data = {'carrier_freq': carr_freq,
                            'rx_gain_ctrl_bb1': rx_1,
                            'rx_gain_ctrl_bb2': rx_2,
                            'rx_gain_ctrl_bb3': rx_3,
                            'rx_gain_ctrl_bfrf': rx_bfrf}
                self.do_start_meas_drone_rfsoc(msg_data)
            elif cmd == 0x05 and length == 0: # STOPDRONERFSOC
                print(f"[DEBUG]: THIS {self.ID} receives STOPDRONERFSOC cmd")
                self.do_stop_meas_drone_rfsoc()
            elif cmd == 0x06 and length == 0: # FINISHDRONERFSOC
                print(f"[DEBUG]: THIS {self.ID} receives FINISHDRONERFSOC cmd")
                self.do_finish_meas_drone_rfsoc()
            elif cmd == 0x07 and length == 0: # CLOSEDGUI
                print(f"[DEBUG]: THIS {self.ID} receives CLOSEDGUI cmd")
                self.do_closed_gui_action()
            elif cmd == 0x08 and length == 5: # SETREMOTEFMFLAG
                print(f"[DEBUG]: THIS {self.ID} receives SETREMOTEFMFLAG cmd")
                data_bytes = data_bytes[:26] # 3 float64, 2 hex
                x,y,z,fmode,mobility = struct.unpack('dddBB', data_bytes)
                mydata ={'X':x, 'Y':y, 'Z':z, 'FMODE': fmode, 'MOBILITY': mobility}
                self.do_set_remote_fm_flag(data=mydata)
            elif cmd == 0x09 and length == 0: # SETREMOTESTOPFM
                print(f"[DEBUG]: THIS {self.ID} receives SETREMOTESTOPFM cmd")
                self.do_set_remote_stop_fm()
            else:
                print("[WARNING]: cmd not known when decoding.  No action will be done")
        elif message_type == 0x02: # LONG cmd type msg
            if cmd == 0x01: # SETIRF
                print(f"[DEBUG]: THIS {self.ID} receives SETIRF cmd. Time snaps: {length}")
                last = int(4*length*16) # The data type of the array entries is float32 and it will have always 16 beams and variable number of time snapshots
                data_bytes = data_bytes[:last]
                data_array = np.frombuffer(data_bytes, dtype=np.float32)
                data_array = data_array.reshape((length, 16))
                self.do_set_irf_action(data_array)
            else:
                print("[WARNING]: cmd not known when decoding.  No action will be done")
        elif message_type == 0x03: # ANS type
            if cmd == 0x01: # Response to GETGPS
                print(f'[DEBUG]: THIS ({self.ID}) receives ANS to GETGPS cmd')
                data_bytes = data_bytes[:27]
                x,y,z,datum,follow_gimbal, fmode = struct.unpack('dddBBB', data_bytes)
                msg_data = {'X': x, 'Y': y, 'Z': z, 'Datum': datum, 'FOLLOW_GIMBAL': follow_gimbal, 'FMODE': fmode}
                self.process_answer_get_gps(msg_data)
            else:
                print("[WARNING]: cmd not known when decoding.  No action will be done")
        else:
            print("[WARNING]: message_type not known when decoding. No action will be done")

    def encode_message(self, source_id, destination_id, message_type, cmd, data=None):
        """
        Encodes a TCP message to be sent. More information about the specific commands is in the section "Communication Protocol" of the "Manual A2GMeasurements".

        Args:
            source_id (int): identifies the sender node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).
            destination_id (int): identifies the receiver node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).
            message_type (hexadecimal): 0x01, for a short type of message; 0x02, for a long type of message; 0x03, to answer/acknowledge a received request. More information about this is in "Manual A2GMeasurements".
            cmd (hexadecimal): one of the supported requests/commands for each ``message_type``. The list of commands is provided in "Manual A2GMeasurements" (Communication Protocol chapter).
            data (dict, optional): additional data required by the request/command. The particular data sent depends on the ``message_type`` and the ``cmd``. More information on "Manual A2GMeasurements" (Communication Protocol chapter). Defaults to None.
        Returns:
            message (bytes): the bytes object representing the message to be sent.
        """

        if message_type == 0x01: # SHORT type of message
            if cmd == 0x01: # FOLLOWGIMBAL
                data = struct.pack('B', data['FMODE'])
                length = 1
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
            elif cmd == 0x02: # GETGPS
                length = 0
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
            elif cmd == 0x03 and data and len(data) == 4: # SETGIMBAL
                data = struct.pack('fffB', data['YAW'], data['PITCH'], data['ROLL'], data['MODE'])
                length = 4
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
            elif cmd == 0x04 and data and len(data) == 5: # STARTDRONERFSOC
                data = struct.pack('fHHHH', data['carrier_freq'], data['rx_gain_ctrl_bb1'], data['rx_gain_ctrl_bb2'], data['rx_gain_ctrl_bb3'], data['rx_gain_ctrl_bfrf'])
                length = 5
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
            elif cmd == 0x05: # STOPDRONERFSOC
                length = 0
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
            elif cmd == 0x06: # FINISHDRONERFSOC
                length = 0
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
            elif cmd == 0x07: # CLOSEDGUI
                length = 0
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
            elif cmd == 0x08: # SETREMOTEFMFLAG
                data = struct.pack('dddBB', data['X'], data['Y'], data['Z'], data['FMODE'], data['MOBILITY'])
                length = 5
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
            elif cmd == 0x09: # SETREMOTESTOPFM
                length = 0
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
        elif message_type == 0x02: #LONG type message
            if cmd == 0x01: # SETIRF
                if data is None:
                    print("[DEBUG]: An array must be provided")
                    return
                data_bytes = data.tobytes()
                length = len(data)
                message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data_bytes
        elif message_type == 0x03: # ANS message type
            if cmd == 0x01: # Response to GETGPS
                message = struct.pack('dddBBB', data['X'], data['Y'], data['Z'], data['Datum'], data['FOLLOW_GIMBAL'], data['FMODE'])
        else:
            print("[DEBUG]: message_type not known when encoding.")
            return

        return message

    def socket_receive(self, stop_event):
        """
        The communication thread callback. Calls the parser to decode the most recent TCP message received.

        The time between calls of this function is OS and hardware dependent.

        As both nodes can send and receive messages, this thread

        Args:
            stop_event (threading.Event): when this is set, this function has nothing to execute.
        """

        # Polling policy for detecting if there has been any message sent.
        # As th thread is scheduled often in the order of ms, this implementation will raise an exception (if nothing is send) quite often
        while not stop_event.is_set():
            try:
                # Send everything in a json serialized packet
                if self.ID == 'GROUND':
                    data = self.a2g_conn.recv(4096) # Up to 1 message of 63 rows and 16 cols of float32 entries
                elif self.ID == 'DRONE':
                    data = self.socket.recv(4096)
                if data:
                    if self.DBG_LVL_0:
                        print('\n[DEBUG_0]: This is the data received: ', data)
                    #print('[DEBUG]: This is the data received: ', len(data['DATA']), len(data['DATA'][0]))
                    self.decode_message(data)
                else:
                    if self.DBG_LVL_0:
                        print('\n[DEBUG_0]: "data" in "if data" in "socket_receive" is None')
            # i.e.  Didn't receive anything
            except Exception as e:
                # Handle the assumed connection lost
                if self.rxEmptySockCounter > self.MAX_NUM_RX_EMPTY_SOCKETS:
                    print('\n[WARNING]:SOCKETS HAVE BEEN EMPTY FOR LONG TIME. DRONE MUST COME CLOSER ', e)
                    self.rxEmptySockCounter = 0

                self.rxEmptySockCounter = self.rxEmptySockCounter + 1

                #traceback.print_exc()
                '''
                Types of known errors:
                1. 'timed out'

                *This error is reported in the client but not in the server. Happens when the client hasn't received anything in a while, so that 'recv' function raises the exception.
                *The conn is open and if any node send something again the other node will receive it
                '''

                if self.DBG_LVL_0:
                    print('[SOCKET RECEIVE EXCEPTION]: ', e)

    def socket_send_cmd(self, type_cmd=None, data=None):
        """
        Creates a message by the name of the request/command. Wrapper to ``encode_message``.

        Args:
            type_cmd (int, optional): refers to the ``cmd`` parameter in ``encode_message``. Defaults to None.
            data (int, optional): refers to the ``data`` parameter in ``encode_message``. Defaults to None.
        """

        if type_cmd == 'SETGIMBAL':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x03, data=data)
        elif type_cmd == 'FOLLOWGIMBAL':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x01, data=data)
        elif type_cmd == 'GETGPS':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x02)
        elif type_cmd == 'CLOSEDGUI':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x07)
        elif type_cmd == 'STARTDRONERFSOC':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x04, data=data)
        elif type_cmd == 'STOPDRONERFSOC':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x05)
        elif type_cmd == 'FINISHDRONERFSOC':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x06)
        elif type_cmd == 'SETIRF':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x02, cmd=0x01, data=data)
        elif type_cmd == 'SETREMOTEFMFLAG':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x08, data=data)
        elif type_cmd == 'SETREMOTESTOPFM':
            frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x09)
        if self.ID == 'DRONE':
            self.socket.sendall(frame)
        elif self.ID == 'GROUND':
            self.a2g_conn.sendall(frame)
        print(f"[DEBUG]: This {self.ID} node sends {type_cmd} cmd")

    def az_rot_gnd_gimbal_toggle_sig_generator(self, Naz, meas_time=10, filename=None):
        """
        Rotates the ground gimbal into "Naz" azimuth steps, while stopping at each angle step, to turn on the signal generator,
        wait for it "meas_time"[seconds] to send signal, and turn it off again.

        Args:
            az_now (int): angle where to start the count. It lies between -1800 and 1800
            Naz (int): number of sectors in azimuth circle
        """

        def fcn_to_execute(state):
            """
            This local function template must be replaced with the instruction to execute when the ground gimbal
            stops for 'meas_time' seconds in a given position. 

            The instruction to execute has to have 2 states: 
                'On') What to execute when ground gimbal just stopped at a new position
                'Off') What to execute when 'meas_time' finishes, and ground gimbal must start again to move to the next position

            Args:
                state (str): ``On`` or ``Off``
            """

            if state == 'On': # 'On' state
                #self.inst.write('RF1\n')
                print('\nOn state... just print')
            elif state == 'Off': # 'Off' state
                #self.inst.write('RF0\n')
                print('\nOff state... just print')
            else:
                print('\n[ERROR]: function to execute must toggle between two states')

        aux_ang_buff = []
        file_to_save = []
        if self.IsGimbal!=0 and self.IsGPS:

            if self.DBG_LVL_1:
                # Remember that reques_current_position is a blocking function
                self.myGimbal.request_current_position()
                az_now = int(round(self.myGimbal.yaw))*10
                pitch_now = int(round(self.myGimbal.pitch))*10
                print('\nYAW NOW: ', az_now, ' PITCH NOW: ', pitch_now)

            ang_step = int(3600/Naz)
            for i in range(Naz):                
                self.myGimbal.setPosControl(yaw=ang_step, roll=0, pitch=0, ctrl_byte=0x00)

                # 1. Sleep until ground gimbal reaches the position, before the instruction gets executed
                # Approximate gimbal speed of 56 deg/s: Max angular movement is 1800 which is done in 3.5 at the actual speed 
                time.sleep(self.myGimbal.TIME2MOVE_180_DEG_YAW) 

                # 2. Execute instruction state 1
                fcn_to_execute('On')

                if self.DBG_LVL_1:
                    print('\n[WARNING]: in iteration ' + str(i+1) + ' of ' + str(Naz)  +', instruction executed, now block thread for ' + str(meas_time) + '  [s]')

                # 3. Sleep for 'meas_time', waiting for instruction to be executed
                time.sleep(meas_time)

                # 4. Execute instruction state 0
                fcn_to_execute('Off')

                # 5. Get last gps coordinates and save them with gimbal info
                coords = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

                if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                    print('\n[ERROR]: gps sbf stream not started or not a single entry in buffer yet')
                    return 
                elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
                    print('\n[ERROR]: gps stream started but not enough satellites or strong multipath propagation interference')
                    return 
                else:                    
                    self.myGimbal.request_current_position()

                    coords['GROUND_GIMBAL_YAW'] = self.myGimbal.yaw
                    coords['GROUND_GIMBAL_PITCH'] = self.myGimbal.pitch
                    file_to_save.append(coords)
        else:
            print('\n[ERROR]: To call this function, IsGimbal and IsGPS have to be set')
            print('\n[WARNING]: No file with coordinates and gimbal yaw, pitch saved')
            return 

        file_to_save = json.dumps(file_to_save)
        fid = open(filename + '.json', 'w') # this overwrites the file
        fid.write(file_to_save)
        fid.close()

        if self.DBG_LVL_1:
            print('\nFile ' + filename + ' saved')

        for i in range(Naz):
            self.myGimbal.setPosControl(yaw=-ang_step, pitch=0, roll=0, ctrl_byte=0x00)
            time.sleep(self.myGimbal.TIME2MOVE_180_DEG_YAW)

    def HelperStartA2GCom(self, PORT=10000):
        """
        Starts the socket binding, listening and accepting for server side, or connecting for client side. The ground node works as the server while the drone as the client.

        Creates and starts the thread handling the socket messages.

        Args:
            PORT (int, optional): TCP port. Defaults to 10000.
        """

        socket_poll_cnt = 1

        # If we know for sure that there will be a client request for connection, we can keep this number low
        MAX_NUM_SOCKET_POLLS = 100

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

        # We need to use a timeout, because otherwise socket.accept() will block the GUI
        self.socket.settimeout(5) 

        # CLIENT
        if self.ID == 'DRONE':
            self.socket.connect((self.SERVER_ADDRESS, PORT))
            if self.DBG_LVL_1:
                print('CONNECTION ESTABLISHED with SERVER ', self.SERVER_ADDRESS)

        # SERVER
        elif self.ID == 'GROUND':            

            # Bind the socket to the port
            self.socket.bind(('', PORT))

            # Listen for incoming connections. As there is one and only one client, we don't need a loop of ``socket.listen()`` calls.
            self.socket.listen()

            # There is no need for an endless loop
            while(socket_poll_cnt < MAX_NUM_SOCKET_POLLS):
                try: 
                    # Blocks until timeout
                    a2g_connection, client_address = self.socket.accept()
                except Exception as es:
                    print("[DEBUG]: No client has been seen there. Poll again for a connection. POLL NUMBER: ", socket_poll_cnt)
                    socket_poll_cnt += 1
                else:
                    break    

            if self.DBG_LVL_1:
                print('CONNECTION ESTABLISHED with CLIENT ', client_address)

            self.a2g_conn = a2g_connection
            self.CLIENT_ADDRESS = client_address

        # This runs a thread that constantly checks for received messages
        # If there are no messages thre will be an error
        # The error might be because there is no sent package(but there is still connection) or because there is no connection anymore
        self.event_stop_thread_helper = threading.Event()
        thread_rx_helper = threading.Thread(target=self.socket_receive, args=(self.event_stop_thread_helper,))
        thread_rx_helper.start()

    def HelperA2GStopCom(self, DISC_WHAT='ALL', stream=1):
        """
        Stops connection with all the devices or the specified ones in the variable 'DISC_WHAT.

        When called, no matter which is the value of ``DISC_WHAT``, always close the TCP socket.

        Args:
            DISC_WHAT (str, optional): specifies with which/s device/s the connection must be ended. Options are: ``GIMBAL``, ``GPS``, ``RFSOC``, ``SG``, ``ALL``. Defaults to ``ALL``.
            stream (int, optional): gps stream to be closed. Assuming there is only one gps stream created at ``__init__`` of this class (which is the default operation) when creating the instance of the ``GpsSignaling`` class, this will close all the gps streams. Defaults to 1.
        """

        try:   
            self.event_stop_thread_helper.set()

            if self.ID == 'DRONE':
                if hasattr(self, 'socket'):
                    self.socket.close()
            elif self.ID == 'GROUND':
                if hasattr(self, 'a2g_conn'):
                    self.a2g_conn.close()
        except:
            print('\n[DEBUG]: ERROR closing connection: probably NO SOCKET created')         

        if type(DISC_WHAT) == list:
            for i in DISC_WHAT:
                if self.IsGimbal!=0 and (i == 'GIMBAL'):  
                    self.myGimbal.stop_thread_gimbal()
                    print('[DEBUG]: Disconnecting gimbal')
                    time.sleep(0.05)
                    if self.ID == 'GROUND':
                        self.myGimbal.actual_bus.shutdown()

                if self.IsGPS and (i == 'GPS'):  
                    for stream_info in self.mySeptentrioGPS.stream_info:
                        if int(stream) == int(stream_info['stream_number']):
                            msg_type = stream_info['msg_type']
                            interface = stream_info['interface']

                        self.mySeptentrioGPS.stop_gps_data_retrieval(stream_number=stream, msg_type=msg_type, interface=interface)
                        print('\n[DEBUG]: Stoping GPS stream')
                        self.mySeptentrioGPS.stop_thread_gps()      

                if self.IsSignalGenerator and (i == 'SG'):
                    self.inst.write('RF0\n')   

                if self.IsRFSoC and (i == 'RFSOC'):
                    self.myrfsoc.radio_control.close()
                    self.myrfsoc.radio_data.close()
        else: # backwards compatibility
            if self.IsGimbal!=0 and (DISC_WHAT=='ALL' or DISC_WHAT == 'GIMBAL'):  
                self.myGimbal.stop_thread_gimbal()
                print('\n[DEBUG]: Disconnecting gimbal')
                time.sleep(0.05)
                if self.ID == 'GROUND':
                    self.myGimbal.actual_bus.shutdown()

            if self.IsGPS and (DISC_WHAT=='ALL' or DISC_WHAT == 'GPS'):  
                for stream_info in self.mySeptentrioGPS.stream_info:
                    if int(stream) == int(stream_info['stream_number']):
                        msg_type = stream_info['msg_type']
                        interface = stream_info['interface']

                self.mySeptentrioGPS.stop_gps_data_retrieval(stream_number=stream, msg_type=msg_type, interface=interface)
                print('\n[DEBUG]: Stoping GPS stream')
                self.mySeptentrioGPS.stop_thread_gps()      

            if self.IsSignalGenerator and (DISC_WHAT=='ALL' or DISC_WHAT == 'SG'): 
                self.inst.write('RF0\n')   

            if self.IsRFSoC and (DISC_WHAT=='ALL' or DISC_WHAT == 'RFSOC'):
                self.myrfsoc.radio_control.close()
                self.myrfsoc.radio_data.close()

HelperA2GStopCom(DISC_WHAT='ALL', stream=1)

Stops connection with all the devices or the specified ones in the variable 'DISC_WHAT.

When called, no matter which is the value of DISC_WHAT, always close the TCP socket.

Parameters:

Name	Type	Description	Default
DISC_WHAT	str	specifies with which/s device/s the connection must be ended. Options are: GIMBAL, GPS, RFSOC, SG, ALL. Defaults to ALL.	'ALL'
stream	int	gps stream to be closed. Assuming there is only one gps stream created at __init__ of this class (which is the default operation) when creating the instance of the GpsSignaling class, this will close all the gps streams. Defaults to 1.	1

Source code in a2gmeasurements.py

def HelperA2GStopCom(self, DISC_WHAT='ALL', stream=1):
    """
    Stops connection with all the devices or the specified ones in the variable 'DISC_WHAT.

    When called, no matter which is the value of ``DISC_WHAT``, always close the TCP socket.

    Args:
        DISC_WHAT (str, optional): specifies with which/s device/s the connection must be ended. Options are: ``GIMBAL``, ``GPS``, ``RFSOC``, ``SG``, ``ALL``. Defaults to ``ALL``.
        stream (int, optional): gps stream to be closed. Assuming there is only one gps stream created at ``__init__`` of this class (which is the default operation) when creating the instance of the ``GpsSignaling`` class, this will close all the gps streams. Defaults to 1.
    """

    try:   
        self.event_stop_thread_helper.set()

        if self.ID == 'DRONE':
            if hasattr(self, 'socket'):
                self.socket.close()
        elif self.ID == 'GROUND':
            if hasattr(self, 'a2g_conn'):
                self.a2g_conn.close()
    except:
        print('\n[DEBUG]: ERROR closing connection: probably NO SOCKET created')         

    if type(DISC_WHAT) == list:
        for i in DISC_WHAT:
            if self.IsGimbal!=0 and (i == 'GIMBAL'):  
                self.myGimbal.stop_thread_gimbal()
                print('[DEBUG]: Disconnecting gimbal')
                time.sleep(0.05)
                if self.ID == 'GROUND':
                    self.myGimbal.actual_bus.shutdown()

            if self.IsGPS and (i == 'GPS'):  
                for stream_info in self.mySeptentrioGPS.stream_info:
                    if int(stream) == int(stream_info['stream_number']):
                        msg_type = stream_info['msg_type']
                        interface = stream_info['interface']

                    self.mySeptentrioGPS.stop_gps_data_retrieval(stream_number=stream, msg_type=msg_type, interface=interface)
                    print('\n[DEBUG]: Stoping GPS stream')
                    self.mySeptentrioGPS.stop_thread_gps()      

            if self.IsSignalGenerator and (i == 'SG'):
                self.inst.write('RF0\n')   

            if self.IsRFSoC and (i == 'RFSOC'):
                self.myrfsoc.radio_control.close()
                self.myrfsoc.radio_data.close()
    else: # backwards compatibility
        if self.IsGimbal!=0 and (DISC_WHAT=='ALL' or DISC_WHAT == 'GIMBAL'):  
            self.myGimbal.stop_thread_gimbal()
            print('\n[DEBUG]: Disconnecting gimbal')
            time.sleep(0.05)
            if self.ID == 'GROUND':
                self.myGimbal.actual_bus.shutdown()

        if self.IsGPS and (DISC_WHAT=='ALL' or DISC_WHAT == 'GPS'):  
            for stream_info in self.mySeptentrioGPS.stream_info:
                if int(stream) == int(stream_info['stream_number']):
                    msg_type = stream_info['msg_type']
                    interface = stream_info['interface']

            self.mySeptentrioGPS.stop_gps_data_retrieval(stream_number=stream, msg_type=msg_type, interface=interface)
            print('\n[DEBUG]: Stoping GPS stream')
            self.mySeptentrioGPS.stop_thread_gps()      

        if self.IsSignalGenerator and (DISC_WHAT=='ALL' or DISC_WHAT == 'SG'): 
            self.inst.write('RF0\n')   

        if self.IsRFSoC and (DISC_WHAT=='ALL' or DISC_WHAT == 'RFSOC'):
            self.myrfsoc.radio_control.close()
            self.myrfsoc.radio_data.close()

HelperStartA2GCom(PORT=10000)

Starts the socket binding, listening and accepting for server side, or connecting for client side. The ground node works as the server while the drone as the client.

Creates and starts the thread handling the socket messages.

Parameters:

Name	Type	Description	Default
PORT	int	TCP port. Defaults to 10000.	10000

Source code in a2gmeasurements.py

def HelperStartA2GCom(self, PORT=10000):
    """
    Starts the socket binding, listening and accepting for server side, or connecting for client side. The ground node works as the server while the drone as the client.

    Creates and starts the thread handling the socket messages.

    Args:
        PORT (int, optional): TCP port. Defaults to 10000.
    """

    socket_poll_cnt = 1

    # If we know for sure that there will be a client request for connection, we can keep this number low
    MAX_NUM_SOCKET_POLLS = 100

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

    # We need to use a timeout, because otherwise socket.accept() will block the GUI
    self.socket.settimeout(5) 

    # CLIENT
    if self.ID == 'DRONE':
        self.socket.connect((self.SERVER_ADDRESS, PORT))
        if self.DBG_LVL_1:
            print('CONNECTION ESTABLISHED with SERVER ', self.SERVER_ADDRESS)

    # SERVER
    elif self.ID == 'GROUND':            

        # Bind the socket to the port
        self.socket.bind(('', PORT))

        # Listen for incoming connections. As there is one and only one client, we don't need a loop of ``socket.listen()`` calls.
        self.socket.listen()

        # There is no need for an endless loop
        while(socket_poll_cnt < MAX_NUM_SOCKET_POLLS):
            try: 
                # Blocks until timeout
                a2g_connection, client_address = self.socket.accept()
            except Exception as es:
                print("[DEBUG]: No client has been seen there. Poll again for a connection. POLL NUMBER: ", socket_poll_cnt)
                socket_poll_cnt += 1
            else:
                break    

        if self.DBG_LVL_1:
            print('CONNECTION ESTABLISHED with CLIENT ', client_address)

        self.a2g_conn = a2g_connection
        self.CLIENT_ADDRESS = client_address

    # This runs a thread that constantly checks for received messages
    # If there are no messages thre will be an error
    # The error might be because there is no sent package(but there is still connection) or because there is no connection anymore
    self.event_stop_thread_helper = threading.Event()
    thread_rx_helper = threading.Thread(target=self.socket_receive, args=(self.event_stop_thread_helper,))
    thread_rx_helper.start()

__init__(ID, SERVER_ADDRESS, DBG_LVL_0=False, DBG_LVL_1=False, IsGimbal=False, IsGPS=False, IsSignalGenerator=False, IsRFSoC=False, rfsoc_static_ip_address=None, F0=None, L0=None, SPEED=0, GPS_Stream_Interval='msec500', AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN=0.001, operating_freq=57510000000.0, heading_offset=0)

Creates instances of classes GimbalRS2 (or GimbalGremsyH16), GpsSignaling, RFSoCRemoteControlFromHost to control these devices.

Parameters:

Name	Type	Description	Default
ID	str	either 'DRONE' or 'GND'.	required
SERVER_ADDRESS	str	the IP address of the ground station.	required
DBG_LVL_0	bool	if set, prints some low-level messages usefull for debugging. Defaults to False.	False
DBG_LVL_1	bool	if set, prints some higher-level messages usefull for debugging. Defaults to False.	False
IsGimbal	bool	0 or FALSE, when no gimbal is physically connected to this host computer; 1, when a Ronin RS2 is physically connected; 2, when a Gremsy H16 is physically connected. Defaults to False.	False
IsGPS	bool	True if a gps is physically connected to this host computer. False otherwise. Defaults to False.	False
IsSignalGenerator	bool	True if a signal generator controlled by pyvisa commands is physically connected to this host computer. False otherwise. Defaults to False.	False
IsRFSoC	bool	True if an RFSoC is physically connected to this host computer. False otherwise. Defaults to False.	False
rfsoc_static_ip_address	str	IP address of the RFSoC connected to this host computer. Defaults to None.	None
L0	float	parameter of the signal generator. Defaults to None.	None
SPEED	int	the speed of the node in m/s. If this node is GROUND it should be 0 (gnd node does not move) as it is by default. This parameter ONLY incides in raising a warning debug print when the speed of the node is higher than the time difference between consecutive SBF sentences. NOT a crutial parameter at all. Stays here for back compatibility. Defaults to 0.	0
GPS_Stream_Interval	str	time interval used for the retrieving of the configured SBF sentences in Septentrio's receiver connected to this host computer. A list of available options is shown in start_gps_data_retrieval of class GpsSignaling. Defaults to 'msec500'.	'msec500'
AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN	float	approximated time between calls of the communication thread. This parameter is used in conjunction with MAX_TIME_EMPTY_SOCKETS to raise an exception when neither side of the communication link is sending any message. Unfortunately, this is a very simple estimate, since the actual time between calls depends on many factors and is does not remain constant between calls. Defaults to 0.001.	0.001
operating_freq	_type_	operating frequency of the Sivers RF-frontend. The range of defined frequencies is defined in the "User Manual EVK06002" of the Sivers EVK (57-71 GHz). Defaults to 57.51e9.	57510000000.0
heading_offset	int	heading offset (check its definition in the GpsSignaling.setHeadingOffset method). Defaults to 0.	0

Source code in a2gmeasurements.py

def __init__(self, ID, SERVER_ADDRESS, 
             DBG_LVL_0=False, DBG_LVL_1=False, 
             IsGimbal=False, IsGPS=False, IsSignalGenerator=False, IsRFSoC=False,
             rfsoc_static_ip_address=None, #uses the default ip_adress
             F0=None, L0=None,
             SPEED=0,
             GPS_Stream_Interval='msec500', AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN=0.001,
             operating_freq=57.51e9,
             heading_offset=0):
    """
    Creates instances of classes ``GimbalRS2`` (or ``GimbalGremsyH16``), ``GpsSignaling``, ``RFSoCRemoteControlFromHost`` to control these devices.

    Args:
        ID (str): either 'DRONE' or 'GND'.
        SERVER_ADDRESS (str): the IP address of the ground station.
        DBG_LVL_0 (bool, optional): if set, prints some low-level messages usefull for debugging. Defaults to False.
        DBG_LVL_1 (bool, optional): if set, prints some higher-level messages usefull for debugging. Defaults to False.
        IsGimbal (bool, optional): 0 or FALSE, when no gimbal is physically connected to this host computer; 1, when a Ronin RS2 is physically connected; 2, when a Gremsy H16 is physically connected. Defaults to False.
        IsGPS (bool, optional): True if a gps is physically connected to this host computer. False otherwise. Defaults to False.
        IsSignalGenerator (bool, optional): True if a signal generator controlled by pyvisa commands is physically connected to this host computer. False otherwise. Defaults to False.
        IsRFSoC (bool, optional): True if an RFSoC is physically connected to this host computer. False otherwise. Defaults to False.
        rfsoc_static_ip_address (str, optional): IP address of the RFSoC connected to this host computer. Defaults to None.
        L0 (float, optional): parameter of the signal generator. Defaults to None.
        SPEED (int, optional): the speed of the node in m/s. If this node is GROUND it should be 0 (gnd node does not move) as it is by default. This parameter ONLY incides in raising a warning debug print when the speed of the node is higher than the time difference between consecutive SBF sentences. NOT a crutial parameter at all. Stays here for back compatibility. Defaults to 0.
        GPS_Stream_Interval (str, optional): time interval used for the retrieving of the configured SBF sentences in Septentrio's receiver connected to this host computer. A list of available options is shown in ``start_gps_data_retrieval`` of class ``GpsSignaling``. Defaults to 'msec500'.
        AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN (float, optional): approximated time between calls of the communication thread. This parameter is used in conjunction with ``MAX_TIME_EMPTY_SOCKETS`` to raise an exception when neither side of the communication link is sending any message. Unfortunately, this is a very simple estimate, since the actual time between calls depends on many factors and is does not remain constant between calls. Defaults to 0.001.
        operating_freq (_type_, optional): operating frequency of the Sivers RF-frontend. The range of defined frequencies is defined in the "User Manual EVK06002" of the Sivers EVK (57-71 GHz). Defaults to 57.51e9.
        heading_offset (int, optional): heading offset (check its definition in the ``GpsSignaling.setHeadingOffset`` method). Defaults to 0.
    """

    self.AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN = AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN
    self.MAX_TIME_EMPTY_SOCKETS = 20 # in [s]
    self.MAX_NUM_RX_EMPTY_SOCKETS = round(self.MAX_TIME_EMPTY_SOCKETS / self.AVG_CALLBACK_TIME_SOCKET_RECEIVE_FCN)
    self.rxEmptySockCounter = 0

    self.ID = ID
    self.SERVER_ADDRESS = SERVER_ADDRESS  
    self.SOCKET_BUFFER = []
    self.DBG_LVL_0 = DBG_LVL_0
    self.DBG_LVL_1 = DBG_LVL_1
    self.IsGimbal = IsGimbal
    self.IsGPS = IsGPS
    self.IsRFSoC = IsRFSoC
    self.IsSignalGenerator = IsSignalGenerator
    self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD = -7.5e3 
    self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM = -8.5e3
    self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY = -9.5e3
    self.SPEED_NODE = SPEED # m/s
    self.CONN_MUST_OVER_FLAG = False # Usefull for drone side, as its script will poll for looking if this is True
    self.PAP_TO_PLOT = []
    self.drone_fm_flag = False

    print(IsGPS, self.IsGPS)

    if IsRFSoC:
        self.myrfsoc = RFSoCRemoteControlFromHost(operating_freq=operating_freq, rfsoc_static_ip_address=rfsoc_static_ip_address)
        print("[DEBUG]: Created RFSoC class")
    if IsGimbal == 1: # By default, the TRUE value is GimbalRS2
        self.myGimbal = GimbalRS2()
        self.myGimbal.start_thread_gimbal()
        time.sleep(0.5)
        print("[DEBUG]: Created Gimbal class")
    elif IsGimbal == 2:
        self.myGimbal = GimbalGremsyH16()
        self.myGimbal.start_thread_gimbal()
        print("[DEBUG]: Created Gimbal class")
    else: # IsGimbal = False
        print("[DEBUG]: No gimbal class is created")
    if IsGPS:
        self.mySeptentrioGPS = GpsSignaling(DBG_LVL_2=True, DBG_LVL_1=False, DBG_LVL_0=False)
        print("[DEBUG]: Created GPS class")
        self.mySeptentrioGPS.serial_connect()

        if self.mySeptentrioGPS.GPS_CONN_SUCCESS:
            self.mySeptentrioGPS.serial_instance.reset_input_buffer()

            # Set the heading offset if any
            self.mySeptentrioGPS.setHeadingOffset(heading_offset)

            if self.ID == 'DRONE':
                self.mySeptentrioGPS.start_gps_data_retrieval(stream_number=1,  msg_type='SBF', interval=GPS_Stream_Interval, sbf_type='+PVTCartesian+AttEuler')
            elif self.ID == 'GROUND':
                self.mySeptentrioGPS.start_gps_data_retrieval(stream_number=1,  msg_type='SBF', interval=GPS_Stream_Interval, sbf_type='+PVTCartesian+AttEuler')
            print("[DEBUG]: started gps stream")
            #self.mySeptentrioGPS.start_gps_data_retrieval(msg_type='NMEA', nmea_type='GGA', interval='sec1')
            self.mySeptentrioGPS.start_thread_gps()
            time.sleep(0.5)
    if IsSignalGenerator:
        rm = pyvisa.ResourceManager()
        inst = rm.open_resource('GPIB0::19::INSTR')
        self.inst = inst
        self.inst.write('F0 ' + str(F0) + ' GH\n')
        self.inst.write('L0 ' + str(L0)+ ' DM\n')
        time.sleep(0.5)

az_rot_gnd_gimbal_toggle_sig_generator(Naz, meas_time=10, filename=None)

Rotates the ground gimbal into "Naz" azimuth steps, while stopping at each angle step, to turn on the signal generator, wait for it "meas_time"[seconds] to send signal, and turn it off again.

Parameters:

Name	Type	Description	Default
az_now	int	angle where to start the count. It lies between -1800 and 1800	required
Naz	int	number of sectors in azimuth circle	required

Source code in a2gmeasurements.py

def az_rot_gnd_gimbal_toggle_sig_generator(self, Naz, meas_time=10, filename=None):
    """
    Rotates the ground gimbal into "Naz" azimuth steps, while stopping at each angle step, to turn on the signal generator,
    wait for it "meas_time"[seconds] to send signal, and turn it off again.

    Args:
        az_now (int): angle where to start the count. It lies between -1800 and 1800
        Naz (int): number of sectors in azimuth circle
    """

    def fcn_to_execute(state):
        """
        This local function template must be replaced with the instruction to execute when the ground gimbal
        stops for 'meas_time' seconds in a given position. 

        The instruction to execute has to have 2 states: 
            'On') What to execute when ground gimbal just stopped at a new position
            'Off') What to execute when 'meas_time' finishes, and ground gimbal must start again to move to the next position

        Args:
            state (str): ``On`` or ``Off``
        """

        if state == 'On': # 'On' state
            #self.inst.write('RF1\n')
            print('\nOn state... just print')
        elif state == 'Off': # 'Off' state
            #self.inst.write('RF0\n')
            print('\nOff state... just print')
        else:
            print('\n[ERROR]: function to execute must toggle between two states')

    aux_ang_buff = []
    file_to_save = []
    if self.IsGimbal!=0 and self.IsGPS:

        if self.DBG_LVL_1:
            # Remember that reques_current_position is a blocking function
            self.myGimbal.request_current_position()
            az_now = int(round(self.myGimbal.yaw))*10
            pitch_now = int(round(self.myGimbal.pitch))*10
            print('\nYAW NOW: ', az_now, ' PITCH NOW: ', pitch_now)

        ang_step = int(3600/Naz)
        for i in range(Naz):                
            self.myGimbal.setPosControl(yaw=ang_step, roll=0, pitch=0, ctrl_byte=0x00)

            # 1. Sleep until ground gimbal reaches the position, before the instruction gets executed
            # Approximate gimbal speed of 56 deg/s: Max angular movement is 1800 which is done in 3.5 at the actual speed 
            time.sleep(self.myGimbal.TIME2MOVE_180_DEG_YAW) 

            # 2. Execute instruction state 1
            fcn_to_execute('On')

            if self.DBG_LVL_1:
                print('\n[WARNING]: in iteration ' + str(i+1) + ' of ' + str(Naz)  +', instruction executed, now block thread for ' + str(meas_time) + '  [s]')

            # 3. Sleep for 'meas_time', waiting for instruction to be executed
            time.sleep(meas_time)

            # 4. Execute instruction state 0
            fcn_to_execute('Off')

            # 5. Get last gps coordinates and save them with gimbal info
            coords = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

            if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                print('\n[ERROR]: gps sbf stream not started or not a single entry in buffer yet')
                return 
            elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
                print('\n[ERROR]: gps stream started but not enough satellites or strong multipath propagation interference')
                return 
            else:                    
                self.myGimbal.request_current_position()

                coords['GROUND_GIMBAL_YAW'] = self.myGimbal.yaw
                coords['GROUND_GIMBAL_PITCH'] = self.myGimbal.pitch
                file_to_save.append(coords)
    else:
        print('\n[ERROR]: To call this function, IsGimbal and IsGPS have to be set')
        print('\n[WARNING]: No file with coordinates and gimbal yaw, pitch saved')
        return 

    file_to_save = json.dumps(file_to_save)
    fid = open(filename + '.json', 'w') # this overwrites the file
    fid.write(file_to_save)
    fid.close()

    if self.DBG_LVL_1:
        print('\nFile ' + filename + ' saved')

    for i in range(Naz):
        self.myGimbal.setPosControl(yaw=-ang_step, pitch=0, roll=0, ctrl_byte=0x00)
        time.sleep(self.myGimbal.TIME2MOVE_180_DEG_YAW)

decode_message(data)

Parses an incoming TCP message and calls the appropriate function to handle it.

This function is called by socket_receive (the communication thread callback).

Parameters:

Name	Type	Description	Default
data	bytes	raw data to be decoded	required

Source code in a2gmeasurements.py

def decode_message(self, data):
    """
    Parses an incoming TCP message and calls the appropriate function to handle it. 

    This function is called by ``socket_receive`` (the communication thread callback).

    Args:
        data (bytes): raw data to be decoded
    """

    source_id, destination_id, message_type, cmd, length = struct.unpack('BBBBB', data[:5])
    data_bytes = data[5:]

    if message_type == 0x01: # SHORT cmd type message
        if cmd == 0x01 and length == 1: # FOLLOWGIMBAL
            print(f"[DEBUG]: THIS {self.ID} receives FOLLOWGIMBAL cmd")
            data_bytes = data_bytes[:1]
            self.do_follow_mode_gimbal(fmode=data_bytes)
        elif cmd == 0x02 and length == 0: # GETGPS
            print(f"[DEBUG]: THIS {self.ID} receives GETGPS cmd")
            self.do_getgps_action()
        elif cmd == 0x03 and length == 4: # SETGIMBAL
            print(f"[DEBUG]: THIS {self.ID} receives SETGIMBAL cmd")
            data_bytes = data_bytes[:13] # 3 float32 array entries + 1 byte
            yaw, pitch, roll, mode = struct.unpack('fffB', data_bytes)
            self.do_setgimbal_action({'YAW': yaw, 'ROLL': roll, 'PITCH': pitch, 'MODE': mode})
        elif cmd == 0x04 and length == 5: # STARTDRONERFSOC
            print(f"[DEBUG]: THIS {self.ID} receives STARTDRONERFSOC cmd")
            data_bytes = data_bytes[:12] # 1 float32 and 4 int16
            carr_freq, rx_1, rx_2, rx_3, rx_bfrf = struct.unpack('fHHHH', data_bytes)

            # float round-error check
            if carr_freq > 70e9 and np.abs(carr_freq-70e9) < 1500: # float round-error of 1.5 kHz
                carr_freq = 70e9
            elif carr_freq < 57.51e9 and np.abs(carr_freq-57.51e9) < 1500: #float round-error of 1.5 kHz
                carr_freq = 57.51e9
            msg_data = {'carrier_freq': carr_freq,
                        'rx_gain_ctrl_bb1': rx_1,
                        'rx_gain_ctrl_bb2': rx_2,
                        'rx_gain_ctrl_bb3': rx_3,
                        'rx_gain_ctrl_bfrf': rx_bfrf}
            self.do_start_meas_drone_rfsoc(msg_data)
        elif cmd == 0x05 and length == 0: # STOPDRONERFSOC
            print(f"[DEBUG]: THIS {self.ID} receives STOPDRONERFSOC cmd")
            self.do_stop_meas_drone_rfsoc()
        elif cmd == 0x06 and length == 0: # FINISHDRONERFSOC
            print(f"[DEBUG]: THIS {self.ID} receives FINISHDRONERFSOC cmd")
            self.do_finish_meas_drone_rfsoc()
        elif cmd == 0x07 and length == 0: # CLOSEDGUI
            print(f"[DEBUG]: THIS {self.ID} receives CLOSEDGUI cmd")
            self.do_closed_gui_action()
        elif cmd == 0x08 and length == 5: # SETREMOTEFMFLAG
            print(f"[DEBUG]: THIS {self.ID} receives SETREMOTEFMFLAG cmd")
            data_bytes = data_bytes[:26] # 3 float64, 2 hex
            x,y,z,fmode,mobility = struct.unpack('dddBB', data_bytes)
            mydata ={'X':x, 'Y':y, 'Z':z, 'FMODE': fmode, 'MOBILITY': mobility}
            self.do_set_remote_fm_flag(data=mydata)
        elif cmd == 0x09 and length == 0: # SETREMOTESTOPFM
            print(f"[DEBUG]: THIS {self.ID} receives SETREMOTESTOPFM cmd")
            self.do_set_remote_stop_fm()
        else:
            print("[WARNING]: cmd not known when decoding.  No action will be done")
    elif message_type == 0x02: # LONG cmd type msg
        if cmd == 0x01: # SETIRF
            print(f"[DEBUG]: THIS {self.ID} receives SETIRF cmd. Time snaps: {length}")
            last = int(4*length*16) # The data type of the array entries is float32 and it will have always 16 beams and variable number of time snapshots
            data_bytes = data_bytes[:last]
            data_array = np.frombuffer(data_bytes, dtype=np.float32)
            data_array = data_array.reshape((length, 16))
            self.do_set_irf_action(data_array)
        else:
            print("[WARNING]: cmd not known when decoding.  No action will be done")
    elif message_type == 0x03: # ANS type
        if cmd == 0x01: # Response to GETGPS
            print(f'[DEBUG]: THIS ({self.ID}) receives ANS to GETGPS cmd')
            data_bytes = data_bytes[:27]
            x,y,z,datum,follow_gimbal, fmode = struct.unpack('dddBBB', data_bytes)
            msg_data = {'X': x, 'Y': y, 'Z': z, 'Datum': datum, 'FOLLOW_GIMBAL': follow_gimbal, 'FMODE': fmode}
            self.process_answer_get_gps(msg_data)
        else:
            print("[WARNING]: cmd not known when decoding.  No action will be done")
    else:
        print("[WARNING]: message_type not known when decoding. No action will be done")

do_closed_gui_action()

Callback function when this node receives a CLOSEDGUI command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

Sets a flag indicating (the drone node) that it can end its main script, since the GUI was closed by the user at the ground node.

Source code in a2gmeasurements.py

def do_closed_gui_action(self):
    """
    Callback function when this node receives a ``CLOSEDGUI`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    Sets a flag indicating (the drone node) that it can end its main script, since the GUI was closed by the user at the ground node.
    """

    if self.ID == 'DRONE':
        self.CONN_MUST_OVER_FLAG = True

do_finish_meas_drone_rfsoc()

Callback function when this node receives a FINISHDRONERFSOC command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

The purpose is to finish the experiment (as defined in "Manual A2GMeasurements"). When the experiment is finished the GUI allows the user to end (disconnect) the connection between both nodes.

Source code in a2gmeasurements.py

def do_finish_meas_drone_rfsoc(self):
    """
    Callback function when this node receives a ``FINISHDRONERFSOC`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    The purpose is to finish the experiment (as defined in "Manual A2GMeasurements"). When the experiment is finished the GUI allows the user to end (disconnect) the connection between both nodes.
    """
    if self.ID == 'DRONE': # double check that we are in the drone
        print("[DEBUG]: Received REQUEST to FINISH measurement")
        self.myrfsoc.finish_measurement()
        self.STOP_SEND_SETIRF_FLAG = True

do_follow_mode_gimbal(fmode=0)

Callback function when this node receives a FOLLOWGIMBAL command.

The FOLLOWGIMBAL command is sent when the other node asks for this node's GPS information to be able to follow this node's movement.

Parameters:

Name	Type	Description	Default
fmode	hexadecimal	specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.	0

Source code in a2gmeasurements.py

def do_follow_mode_gimbal(self, fmode=0x00):
    """
    Callback function when this node receives a ``FOLLOWGIMBAL`` command.

    The ``FOLLOWGIMBAL`` command is sent when the other node asks for this node's GPS information to be able to follow this node's movement.        

    Args:
        fmode (hexadecimal, optional): specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.
    """

    self.do_getgps_action(follow_mode_gimbal=True, fmode=0x00)

do_getgps_action(follow_mode_gimbal=False, fmode=0)

Callback function when this node receives a GETGPS command.

The GETGPS commmand differentiates from FOLLOWGIMBAL in that when the other node only request GPS information from this node (i.e. for display the coordinates on a panel of the GUI), the follow_mode_gimbal is False as well as the FMODE key of the sent dictionary.

Parameters:

Name	Type	Description	Default
follow_mode_gimbal	bool	True if other node's gimbal must follow this node's movement. Defaults to False.	False
fmode	hexadecimal	specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.	0

Source code in a2gmeasurements.py

def do_getgps_action(self, follow_mode_gimbal=False, fmode=0x00):
    """
    Callback function when this node receives a ``GETGPS`` command.

    The ``GETGPS`` commmand differentiates from ``FOLLOWGIMBAL`` in that when the other node only request GPS information from this node (i.e. for display the coordinates on a panel of the GUI), the ``follow_mode_gimbal`` is False as well as the ``FMODE`` key of the sent dictionary.

    Args:
        follow_mode_gimbal (bool, optional): True if other node's gimbal must follow this node's movement. Defaults to False.
        fmode (hexadecimal, optional): specifies whether the other node shall follow this node's movement in: 0x00, Elevation and azimuth; 0x01, Only elevation; 0x02, Only azimuth. Defaults to 0x00.
    """

    if self.DBG_LVL_1:
        print(f"THIS ({self.ID}) receives a GETGPS command")

    if self.IsGPS:            
        # Only need to send to the OTHER station our last coordinates, NOT heading.
        # Heading info required by the OTHER station is Heading info from the OTHER station

        # It has to send over the socket the geocentric/geodetic coordinates
        # The only way there are no coordinates available is because:
        # 1) Didn't start gps thread with PVTCart and AttEuler type of messages
        # 2) Messages interval is too long and the program executed first than the first message arrived
        # 3) The receiver is not connected to enough satellites or multipath propagation is very strong, so that ERROR == 1

        data_to_send = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

        if data_to_send['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
            # More verbose
            print(f"[WARNING]: This {self.ID} has nothing on GPS buffer")
            return

        elif data_to_send['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
            # More verbose
            print(f"[WARNING]: This {self.ID} does not have GPS or GPS signals are not available")
            return

        if follow_mode_gimbal:
            print('[DEBUG]: Last coordinates retrieved and followgimbal flag set to True to be sent')
            data_to_send['FOLLOW_GIMBAL'] = 0x01
            data_to_send['FMODE'] = fmode

        # data_to_send wont be any of the other error codes, because they are not set for 'what'=='Coordinates'
        else:            
            data_to_send['FOLLOW_GIMBAL'] = 0x02

        if self.ID == 'GROUND':
            frame_to_send = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x03, cmd=0x01, data=data_to_send)
        elif self.ID == 'DRONE':
            frame_to_send = self.encode_message(source_id=0x02, destination_id=0x01, message_type=0x03, cmd=0x01, data=data_to_send)

        if self.DBG_LVL_1:
            print('\n[DEBUG_1]:Received the GETGPS and read the SBF buffer')
        if self.ID == 'GROUND':
            self.a2g_conn.sendall(frame_to_send)
        if self.ID == 'DRONE':
            self.socket.sendall(frame_to_send)

        if self.DBG_LVL_1:
            print('\n[DEBUG_1]: Sent SBF buffer')

    else:
        #print('[WARNING]:ASKED for GPS position but no GPS connected: IsGPS is False')
        1

do_set_irf_action(msg_data)

Callback function when this node receives a SETIRF command.

This comand is unidirectional. It is always sent by the drone node to the ground node.

Receives from the drone a subsampled version of the Power Angular Profile for it to be used by the GUI to continuously plot it in its PAP panel.

Parameters:

Name	Type	Description	Default
msg_data	ndarray	attribute value data_to_visualize from RFSoCRemoteControlFromHost class.	required

Source code in a2gmeasurements.py

def do_set_irf_action(self, msg_data):
    """
    Callback function when this node receives a ``SETIRF`` command.

    This comand is unidirectional. It is always sent by the drone node to the ground node.

    Receives from the drone a subsampled version of the Power Angular Profile for it to be used by the GUI to continuously plot it in its PAP panel.

    Args:
        msg_data (numpy.ndarray): attribute value ``data_to_visualize`` from ``RFSoCRemoteControlFromHost`` class.
    """

    if self.ID == 'GROUND': # double checj that we are in the gnd
        self.PAP_TO_PLOT = np.asarray(msg_data)

do_set_remote_fm_flag(data=None)

Callback function when this node receives a SETREMOTEFMFLAG command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

Sets the drone_fm_flag. When this flag is set, the drone node can start sending FOLLOWGIMBALL commands to the ground node to get ground node's coordinates and be able to follow (drone node) it (ground node).

Parameters:

Name	Type	Description	Default
data	dict	dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'MOBILITY', corresponding to geocentric coordinates. The mentioned keys and their corresponding values refer to the ground node. The coordinates will be available if the ground node MOBILITY is static (0x01). The FMODE key refers to the wheter the drone gimbal follows the ground node in azimuth (0x02), elevation (0x01) or both (0x00) .Defaults to None.	None

Source code in a2gmeasurements.py

def do_set_remote_fm_flag(self, data=None):
    """
    Callback function when this node receives a ``SETREMOTEFMFLAG`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    Sets the ``drone_fm_flag``. When this flag is set, the drone node can start sending ``FOLLOWGIMBALL`` commands to the ground node to get ground node's coordinates and be able to follow (drone node) it (ground node).

    Args:
        data (dict, optional): dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'MOBILITY', corresponding to geocentric coordinates. The mentioned keys and their corresponding values refer to the ground node. The coordinates will be available if the ground node ``MOBILITY`` is ``static`` (0x01). The ``FMODE`` key refers to the wheter the drone gimbal follows the ground node in azimuth (0x02), elevation (0x01) or both (0x00) .Defaults to None.
    """

    if self.ID == 'DRONE':
        self.drone_fm_flag = True
        self.remote_config_for_drone_fm = data

do_set_remote_stop_fm()

Callback function when this node receives a SETREMOTESTOPFM command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

Unsets the drone_fm_flag flag.

Source code in a2gmeasurements.py

def do_set_remote_stop_fm(self):
    """
    Callback function when this node receives a ``SETREMOTESTOPFM`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    Unsets the ``drone_fm_flag`` flag.
    """

    if self.ID == 'DRONE':
        self.drone_fm_flag = False

do_setgimbal_action(msg_data)

Callback function when this node receives a SETGIMBAL command.

Parameters:

Name	Type	Description	Default
msg_data	dict	dictionary with keys 'YAW', 'PITCH' and 'MODE'. The 'YAW' values range between [-1800, 1800]. The 'PITCH' values are restricted (by software) to the interval [-600, 600] to avoid hits between the case and the gimbal. The 'MODE' values are: 0x00, consider 'YAW' and/or 'PITCH' values as relative to the actual position of the gimbal; 0x01, consider 'YAW' and/or 'PITCH' values as relative to the absolute 0 (in both azimuth and elevation) position.	required

Source code in a2gmeasurements.py

def do_setgimbal_action(self, msg_data):
    """
    Callback function when this node receives a ``SETGIMBAL`` command.

    Args:
        msg_data (dict): dictionary with keys 'YAW', 'PITCH' and 'MODE'. The 'YAW' values range between [-1800, 1800]. The 'PITCH' values are restricted (by software) to the interval [-600, 600] to avoid hits between the case and the gimbal. The 'MODE' values are: 0x00, consider 'YAW' and/or 'PITCH' values as relative to the actual position of the gimbal; 0x01, consider 'YAW' and/or 'PITCH' values as relative to the absolute 0 (in both azimuth and elevation) position.
    """

    if self.IsGimbal!=0:
        # Unwrap the dictionary containing the yaw and pitch values to be set.
        #msg_data = json.loads(msg_data)

        # Error checking
        #if 'YAW' not in msg_data or 'PITCH' not in msg_data or 'MODE' not in msg_data:
        if 'YAW' not in msg_data or 'PITCH' not in msg_data:
            if 'MODE' not in msg_data:
                print('[ERROR]: no YAW or PITCH provided')
                return
            else:
                self.myGimbal.change_gimbal_mode(mode=msg_data['MODE'])                    
        elif 'YAW' in msg_data and 'PITCH' in msg_data:
            if float(msg_data['YAW']) > 1800 or float(msg_data['PITCH']) > 600 or float(msg_data['YAW']) < -1800 or float(msg_data['PITCH']) < -600:
                print('[ERROR]: Yaw or pitch angles are outside of range')
                return
            else:
                if self.IsGimbal == 1: # RS2
                    # Cast to int values as a double check, but values are send as numbers and not as strings.
                    self.myGimbal.setPosControl(yaw=int(msg_data['YAW']), roll=0, pitch=int(msg_data['PITCH']), ctrl_byte=msg_data['MODE'])
                if self.IsGimbal == 2: # Gremsy
                    # Cast to int values as a double check, but values are send as numbers and not as strings.
                    self.myGimbal.setPosControl(yaw=float(msg_data['YAW']), pitch=float(msg_data['PITCH']), mode=msg_data['MODE'])
    else:
        print('\n[WARNING]: Action to SET Gimbal not posible cause there is no gimbal: IsGimbal is False')

do_start_meas_drone_rfsoc(msg_data)

Callback function when this node receives a STARTDRONERFSOC command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

The purpose is to start the RFSoC thread (created in RFSoCRemoteControlFromHost class) responsible for retrieving the measured Channel Impulse Response from the RFSoC.

It is assumed that prior to this callback, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.

Parameters:

Name	Type	Description	Default
msg_data	dict	dictionary with keys 'carrier_freq', 'rx_gain_ctrl_bb1', 'rx_gain_ctrl_bb2', 'rx_gain_ctrl_bb3', 'rx_gain_ctrl_bfrf'. More information about these keys can be found in method set_rx_rf from RFSoCRemoteControlFromHost class.	required

Source code in a2gmeasurements.py

def do_start_meas_drone_rfsoc(self, msg_data):
    """
    Callback function when this node receives a ``STARTDRONERFSOC`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    The purpose is to start the RFSoC thread (created in ``RFSoCRemoteControlFromHost`` class) responsible for retrieving the measured Channel Impulse Response from the RFSoC.

    It is assumed that prior to this callback, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.

    Args:
        msg_data (dict): dictionary with keys 'carrier_freq', 'rx_gain_ctrl_bb1', 'rx_gain_ctrl_bb2', 'rx_gain_ctrl_bb3', 'rx_gain_ctrl_bfrf'. More information about these keys can be found in method ``set_rx_rf`` from ``RFSoCRemoteControlFromHost`` class.
    """

    if self.ID == 'DRONE': # double check that we are in the drone
        print("[DEBUG]: Received REQUEST to START measurement")
        self.myrfsoc.start_thread_receive_meas_data(msg_data)
        self.STOP_SEND_SETIRF_FLAG = False

do_stop_meas_drone_rfsoc()

Callback function when this node receives a STOPDRONERFSOC command.

This comand is unidirectional. It is always sent by the ground node to the drone node.

The purpose is to stop the RFSoC thread.

It is assumed that prior to this function, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.

Source code in a2gmeasurements.py

def do_stop_meas_drone_rfsoc(self):
    """
    Callback function when this node receives a ``STOPDRONERFSOC`` command.

    This comand is unidirectional. It is always sent by the ground node to the drone node.

    The purpose is to stop the RFSoC thread.

    It is assumed that prior to this function, the ground rfsoc (tx) has started sending the its sounding signal and there were no issues.
    """
    if self.ID == 'DRONE': # double check that we are in the drone
        print("[DEBUG]: Received REQUEST to STOP measurement")
        self.myrfsoc.stop_thread_receive_meas_data()
        self.STOP_SEND_SETIRF_FLAG = True

encode_message(source_id, destination_id, message_type, cmd, data=None)

Encodes a TCP message to be sent. More information about the specific commands is in the section "Communication Protocol" of the "Manual A2GMeasurements".

Parameters:

Name	Type	Description	Default
source_id	int	identifies the sender node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).	required
destination_id	int	identifies the receiver node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).	required
message_type	hexadecimal	0x01, for a short type of message; 0x02, for a long type of message; 0x03, to answer/acknowledge a received request. More information about this is in "Manual A2GMeasurements".	required
cmd	hexadecimal	one of the supported requests/commands for each message_type. The list of commands is provided in "Manual A2GMeasurements" (Communication Protocol chapter).	required
data	dict	additional data required by the request/command. The particular data sent depends on the message_type and the cmd. More information on "Manual A2GMeasurements" (Communication Protocol chapter). Defaults to None.	None

Returns: message (bytes): the bytes object representing the message to be sent.

Source code in a2gmeasurements.py

def encode_message(self, source_id, destination_id, message_type, cmd, data=None):
    """
    Encodes a TCP message to be sent. More information about the specific commands is in the section "Communication Protocol" of the "Manual A2GMeasurements".

    Args:
        source_id (int): identifies the sender node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).
        destination_id (int): identifies the receiver node with a number (this parameter is provided for -potential- future improvements but does not have any functionality).
        message_type (hexadecimal): 0x01, for a short type of message; 0x02, for a long type of message; 0x03, to answer/acknowledge a received request. More information about this is in "Manual A2GMeasurements".
        cmd (hexadecimal): one of the supported requests/commands for each ``message_type``. The list of commands is provided in "Manual A2GMeasurements" (Communication Protocol chapter).
        data (dict, optional): additional data required by the request/command. The particular data sent depends on the ``message_type`` and the ``cmd``. More information on "Manual A2GMeasurements" (Communication Protocol chapter). Defaults to None.
    Returns:
        message (bytes): the bytes object representing the message to be sent.
    """

    if message_type == 0x01: # SHORT type of message
        if cmd == 0x01: # FOLLOWGIMBAL
            data = struct.pack('B', data['FMODE'])
            length = 1
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
        elif cmd == 0x02: # GETGPS
            length = 0
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
        elif cmd == 0x03 and data and len(data) == 4: # SETGIMBAL
            data = struct.pack('fffB', data['YAW'], data['PITCH'], data['ROLL'], data['MODE'])
            length = 4
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
        elif cmd == 0x04 and data and len(data) == 5: # STARTDRONERFSOC
            data = struct.pack('fHHHH', data['carrier_freq'], data['rx_gain_ctrl_bb1'], data['rx_gain_ctrl_bb2'], data['rx_gain_ctrl_bb3'], data['rx_gain_ctrl_bfrf'])
            length = 5
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
        elif cmd == 0x05: # STOPDRONERFSOC
            length = 0
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
        elif cmd == 0x06: # FINISHDRONERFSOC
            length = 0
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
        elif cmd == 0x07: # CLOSEDGUI
            length = 0
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
        elif cmd == 0x08: # SETREMOTEFMFLAG
            data = struct.pack('dddBB', data['X'], data['Y'], data['Z'], data['FMODE'], data['MOBILITY'])
            length = 5
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data
        elif cmd == 0x09: # SETREMOTESTOPFM
            length = 0
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length)
    elif message_type == 0x02: #LONG type message
        if cmd == 0x01: # SETIRF
            if data is None:
                print("[DEBUG]: An array must be provided")
                return
            data_bytes = data.tobytes()
            length = len(data)
            message = struct.pack('BBBBB', source_id, destination_id, message_type, cmd, length) + data_bytes
    elif message_type == 0x03: # ANS message type
        if cmd == 0x01: # Response to GETGPS
            message = struct.pack('dddBBB', data['X'], data['Y'], data['Z'], data['Datum'], data['FOLLOW_GIMBAL'], data['FMODE'])
    else:
        print("[DEBUG]: message_type not known when encoding.")
        return

    return message

gimbal_follows_drone(heading=None, lat_ground=None, lon_ground=None, height_ground=None, lat_drone=None, lon_drone=None, height_drone=None, fmode=0)

Computes the yaw, pitch and roll angles required to move the gimbal in this node towards the other node.

The caller of this function must guarantee that if self.ID == 'GROUND', the arguments passed to this function are drone coords. The ground coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

The caller of this function must guarantee that if self.ID == 'DRONE', the arguments passed to this function are ground coords. The drone coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

If IsGPS is False (no GPS connected), then heading, lat_ground, lon_ground, height_ground, lat_drone, lon_drone, height_drone must be provided.

In that case, all coordinates provided must be geodetic (lat, lon, alt).

Parameters:

Name	Type	Description	Default
heading	float	angle between [0, 2*pi] (rads) corresponding to the heading of the line between the two antennas connected to Septentrio's receiver in this node. Defaults to None.	None
lat_ground	float	latitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.	None
lon_ground	float	longitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.	None
height_ground	float	height of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.	None
lat_drone	float	latitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.	None
lon_drone	float	longitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.	None
height_drone	float	height of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.	None
fmode	hexadecimal	defines if the gimbal will follow the other node in Azimuth, elevation or both of them. Options are: 0x00, for Azimuth and elevation; 0x01, for Elevation, 0x02, for Azimuth. Defaults to 0x00.	0

Returns:

Name	Type	Description
yaw_to_set	(int, optional)	yaw angle to be set at the gimbal of this node, in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458)
pitch_to_set	(int, optional)	pitch angle to be set at the gimbal of this node, to follow the other node. The actual value is the angle value in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458).

Source code in a2gmeasurements.py

def gimbal_follows_drone(self, heading=None, lat_ground=None, lon_ground=None, height_ground=None, 
                                lat_drone=None, lon_drone=None, height_drone=None, fmode=0x00):
    """
    Computes the yaw, pitch and roll angles required to move the gimbal in this node towards the other node.

    The caller of this function must guarantee that if ``self.ID == 'GROUND'``, the arguments passed to this function are drone coords. The ground coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

    The caller of this function must guarantee that if ``self.ID == 'DRONE'``, the arguments passed to this function are ground coords. The drone coords SHOULD NOT be passed as they will be obtained from this node Septentrio's receiver.

    If ``IsGPS`` is False (no GPS connected), then ``heading``, ``lat_ground``, ``lon_ground``, ``height_ground``, ``lat_drone``, ``lon_drone``, ``height_drone`` must be provided. 

    In that case, all coordinates provided must be geodetic (lat, lon, alt).

    Args:
        heading (float, optional): angle between [0, 2*pi] (rads) corresponding to the heading of the line between the two antennas connected to Septentrio's receiver in this node. Defaults to None.
        lat_ground (float, optional): latitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.
        lon_ground (float, optional): longitude of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Defaults to None.
        height_ground (float, optional): height of the GPS antenna 1 connected to Septentrio's receiver at the GROUND node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.
        lat_drone (float, optional): latitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.
        lon_drone (float, optional): longitude of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Defaults to None.
        height_drone (float, optional): height of the GPS antenna 1 connected to Septentrio's receiver at the DRONE node. Assuming both antennas are placed at the same height, is the altitude (in meters above sea level) of the either of the antennas. Defaults to None.
        fmode (hexadecimal, optional): defines if the gimbal will follow the other node in Azimuth, elevation or both of them. Options are: 0x00, for Azimuth and elevation; 0x01, for Elevation, 0x02, for Azimuth. Defaults to 0x00.

    Returns:
        yaw_to_set (int, optional): yaw angle to be set at the gimbal of this node, in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458)
        pitch_to_set(int, optional): pitch angle to be set at the gimbal of this node, to follow the other node. The actual value is the angle value in degrees multiplied by 10 and rounded to the closest integer (i.e. a yaw to set of 45.78 degrees is returned as the yaw value 458).
    """

    if self.IsGPS:
        if fmode == 0x00 or fmode == 0x02:
            coords, head_info = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Both')

            if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                return self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL
            elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ:
                return self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ
            else:
                heading = head_info['Heading']
                time_tag_heading = head_info['TOW']
                time_tag_coords = coords['TOW']
                datum_coordinates = coords['Datum']
        elif fmode == 0x01:
            coords = self.mySeptentrioGPS.get_last_sbf_buffer_info(what='Coordinates')

            if coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
                return self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL, self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL
            elif coords['X'] == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ:
                return self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ, self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ
            else:
                time_tag_coords = coords['TOW']
                datum_coordinates = coords['Datum']

        '''
        Check if the time difference (ms) between the heading and the coordinates info is less
        than the time it takes the node to move a predefined distance with the actual speed.           
        If the node is not moving (self.SPEED = 0) it means the heading info will be always the same
        and the check is not required.
        '''
        if self.SPEED_NODE > 0:
            time_distance_allowed = 2 # meters
            if  abs(time_tag_coords - time_tag_heading) > (time_distance_allowed/self.SPEED_NODE)*1000:
                print('\n[WARNING]: for the time_distance_allowed the heading info of the grounde node does not correspond to the coordinates')
                return self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD, self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD, self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD

        if self.ID == 'GROUND':
        # Convert Geocentric WGS84 to Geodetic to compute distance and Inverse Transform Forward Azimuth (ITFA) 
            if datum_coordinates == 0:
                lat_ground, lon_ground, height_ground = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'])
            # Geocentric ETRS89
            elif datum_coordinates == 30:
                lat_ground, lon_ground, height_ground = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'], EPSG_GEOCENTRIC=4346)
            else:
                print('\n[ERROR]: Not known geocentric datum')
                return self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM

        elif self.ID == 'DRONE':
            # Convert Geocentric WGS84 to Geodetic to compute distance and Inverse Transform Forward Azimuth (ITFA) 
            if datum_coordinates == 0:
                lat_drone, lon_drone, height_drone = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'])
            # Geocentric ETRS89
            elif datum_coordinates == 30:
                lat_drone, lon_drone, height_drone = geocentric2geodetic(coords['X'], coords['Y'], coords['Z'], EPSG_GEOCENTRIC=4346)
            else:
                print('\n[ERROR]: Not known geocentric datum')
                return self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM, self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM
    # Testing mode
    elif self.IsGPS == False:
        # Both coordinates must be provided and must be in geodetic format
        1

    if (lat_ground is None and lat_drone is None) or (lon_ground is None and lon_drone is None) or (height_ground is None and height_drone is None):
        print("\n[ERROR]: Either ground or drone coordinates MUST be provided")
        return self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY, self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY, self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY       

    if fmode == 0x00: # Azimuth and elevation
        if self.ID == 'DRONE':
            yaw_to_set = azimuth_difference_between_coordinates(heading, lat_drone, lon_drone, lat_ground, lon_ground)
            pitch_to_set = elevation_difference_between_coordinates(lat_drone, lon_drone, height_drone, lat_ground, lon_ground, height_ground)
        elif self.ID == 'GROUND':
            yaw_to_set = azimuth_difference_between_coordinates(heading, lat_ground, lon_ground, lat_drone, lon_drone)
            pitch_to_set = elevation_difference_between_coordinates(lat_ground, lon_ground, height_ground, lat_drone, lon_drone, height_drone)
    elif fmode == 0x02: # Azimuth
        if self.ID == 'DRONE':
            yaw_to_set = azimuth_difference_between_coordinates(heading, lat_drone, lon_drone, lat_ground, lon_ground)
        elif self.ID == 'GROUND':
            yaw_to_set = azimuth_difference_between_coordinates(heading, lat_ground, lon_ground, lat_drone, lon_drone)
    elif fmode == 0x01: # Elevation
        if self.ID == 'DRONE':
            pitch_to_set = elevation_difference_between_coordinates(lat_drone, lon_drone, height_drone, lat_ground, lon_ground, height_ground)
        elif self.ID == 'GROUND':
            pitch_to_set = elevation_difference_between_coordinates(lat_ground, lon_ground, height_ground, lat_drone, lon_drone, height_drone)

    return yaw_to_set, pitch_to_set

process_answer_get_gps(data)

Callback function when this node receives an ANS type of message (the equivalent to an acknowledment) from the other node, after this node sent to the other node a GETGPS or a FOLLOWGIMBAL command.

Parameters:

Name	Type	Description	Default
data	dict	dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'FOLLOW_GIMBAL'. The values of 'X', 'Y', 'Z' are the geocentric coordinates from the other node. 'FMODE' is either 0x00 (Elevation and Azimuth), 0x01 (Elevation) or 0x02 (Azimuth). 'FOLLOW_GIMBAL' is either True (when the sent command by this node was FOLLOWGIMBAL) or False (when the sent command by this node was GETGPS)	required

Source code in a2gmeasurements.py

def process_answer_get_gps(self, data):
    """
    Callback function when this node receives an ``ANS`` type of message (the equivalent to an acknowledment) from the other node, after this node sent to the other node a ``GETGPS`` or a ``FOLLOWGIMBAL`` command.

    Args:
        data (dict): dictionary with keys 'X', 'Y', 'Z', 'FMODE', 'FOLLOW_GIMBAL'. The values of 'X', 'Y', 'Z' are the geocentric coordinates from the other node. 'FMODE' is either 0x00 (Elevation and Azimuth), 0x01 (Elevation) or 0x02 (Azimuth). 'FOLLOW_GIMBAL' is either True (when the sent command by this node was ``FOLLOWGIMBAL``) or False (when the sent command by this node was ``GETGPS``)
    """

    if self.DBG_LVL_1:
        print(f"THIS ({self.ID}) receives protocol ANS")

    if self.ID =='DRONE':
        y_gnd = data['Y']
        x_gnd = data['X']

        datum_coordinates = data['Datum']

        if y_gnd == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL or x_gnd == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
            print('[ERROR]: no GPS coordinates received from DRONE through socket link')
            return

            # THE HEIGHT VALUE IS THE ALTITUDE VALUE OVER THE SEA LEVEL
            # Z is in geocentric coordinates and does not correspond to the actual height:
            # Geocentric WGS84
        if datum_coordinates == 0:
            lat_gnd, lon_gnd, height_gnd = geocentric2geodetic(x_gnd, y_gnd, data['Z'])
            # Geocentric ETRS89
        elif datum_coordinates == 30:
            lat_gnd, lon_gnd, height_gnd = geocentric2geodetic(x_gnd, y_gnd, data['Z'], EPSG_GEOCENTRIC=4346)
        else:
            print('[ERROR]: Not known geocentric datum')
            return

        yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_ground=lat_gnd, lon_ground=lon_gnd, height_ground=height_gnd, fmode=data['FMODE'])

        while ((yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ) or (pitch_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ)):
            yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_ground=lat_gnd, lon_ground=lon_gnd, height_ground=height_gnd, fmode=data['FMODE'])
    elif self.ID == 'GROUND':
        y_drone = data['Y']
        x_drone = data['X']

        datum_coordinates = data['Datum']

        if y_drone == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL or x_drone == self.mySeptentrioGPS.ERR_GPS_CODE_NO_COORD_AVAIL:
            print('[ERROR]: no GPS coordinates received from DRONE through socket link')
            return

            # Z is in geocentric coordinates and does not correspond to the actual height:
            # Geocentric WGS84
        if datum_coordinates == 0:
            lat_drone, lon_drone, height_drone = geocentric2geodetic(x_drone, y_drone, data['Z'])
            # Geocentric ETRS89
        elif datum_coordinates == 30:
            lat_drone, lon_drone, height_drone = geocentric2geodetic(x_drone, y_drone, data['Z'], EPSG_GEOCENTRIC=4346)
        else:
            print('[ERROR]: Not known geocentric datum')
            return

        # This is for GUI GPS panel to show drone coordinates
        self.last_drone_coords_requested = {'LAT': lat_drone, 'LON': lon_drone}

        yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_drone=lat_drone, lon_drone=lon_drone, height_drone=height_drone, fmode=data['FMODE'])

            # If error [yaw, pitch] values because not enough gps buffer entries (but gps already has entries, meaning is working), call again the gimbal_follows_drone method
        while ((yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ) or (pitch_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_SMALL_BUFF_SZ)):
            yaw_to_set, pitch_to_set = self.gimbal_follows_drone(lat_drone=lat_drone, lon_drone=lon_drone, height_drone=height_drone, fmode=data['FMODE'])

    if yaw_to_set == self.ERR_HELPER_CODE_GPS_HEAD_UNRELATED_2_COORD or yaw_to_set == self.ERR_HELPER_CODE_GPS_NOT_KNOWN_DATUM or yaw_to_set == self.ERR_HELPER_CODE_BOTH_NODES_COORDS_CANTBE_EMPTY or yaw_to_set == self.mySeptentrioGPS.ERR_GPS_CODE_BUFF_NULL:
        print('[ERROR]: one of the error codes of gimbal_follows_drone persists')
        print(f"[DEBUG]: This {self.ID} gimbal will NOT follow its pair node due to ERROR")
    else:
        print(f"[DEBUG]: This {self.ID} YAW to set is: {yaw_to_set}, and PITCH to set is: {pitch_to_set}")

        if data['FOLLOW_GIMBAL'] == 0x01: # True, Follow gimbal
            if self.IsGimbal!=0: # There is a gimbal at the node that receives the answer to its command request.
                self.myGimbal.setPosControl(yaw=yaw_to_set, pitch=pitch_to_set) 
                print(f"[DEBUG]: This {self.ID} gimbal WILL follow its pair node as stated by user")
            else:
                print('[WARNING]: No gimbal available, so no rotation will happen')
        elif data['FOLLOW_GIMBAL'] == 0x02: # False
            print(f"[DEBUG]: This {self.ID} gimbal will NOT follow its pair node as stated by user")

socket_receive(stop_event)

The communication thread callback. Calls the parser to decode the most recent TCP message received.

The time between calls of this function is OS and hardware dependent.

As both nodes can send and receive messages, this thread

Parameters:

Name	Type	Description	Default
stop_event	Event	when this is set, this function has nothing to execute.	required

Source code in a2gmeasurements.py

def socket_receive(self, stop_event):
    """
    The communication thread callback. Calls the parser to decode the most recent TCP message received.

    The time between calls of this function is OS and hardware dependent.

    As both nodes can send and receive messages, this thread

    Args:
        stop_event (threading.Event): when this is set, this function has nothing to execute.
    """

    # Polling policy for detecting if there has been any message sent.
    # As th thread is scheduled often in the order of ms, this implementation will raise an exception (if nothing is send) quite often
    while not stop_event.is_set():
        try:
            # Send everything in a json serialized packet
            if self.ID == 'GROUND':
                data = self.a2g_conn.recv(4096) # Up to 1 message of 63 rows and 16 cols of float32 entries
            elif self.ID == 'DRONE':
                data = self.socket.recv(4096)
            if data:
                if self.DBG_LVL_0:
                    print('\n[DEBUG_0]: This is the data received: ', data)
                #print('[DEBUG]: This is the data received: ', len(data['DATA']), len(data['DATA'][0]))
                self.decode_message(data)
            else:
                if self.DBG_LVL_0:
                    print('\n[DEBUG_0]: "data" in "if data" in "socket_receive" is None')
        # i.e.  Didn't receive anything
        except Exception as e:
            # Handle the assumed connection lost
            if self.rxEmptySockCounter > self.MAX_NUM_RX_EMPTY_SOCKETS:
                print('\n[WARNING]:SOCKETS HAVE BEEN EMPTY FOR LONG TIME. DRONE MUST COME CLOSER ', e)
                self.rxEmptySockCounter = 0

            self.rxEmptySockCounter = self.rxEmptySockCounter + 1

            #traceback.print_exc()
            '''
            Types of known errors:
            1. 'timed out'

            *This error is reported in the client but not in the server. Happens when the client hasn't received anything in a while, so that 'recv' function raises the exception.
            *The conn is open and if any node send something again the other node will receive it
            '''

            if self.DBG_LVL_0:
                print('[SOCKET RECEIVE EXCEPTION]: ', e)

socket_send_cmd(type_cmd=None, data=None)

Creates a message by the name of the request/command. Wrapper to encode_message.

Parameters:

Name	Type	Description	Default
type_cmd	int	refers to the cmd parameter in encode_message. Defaults to None.	None
data	int	refers to the data parameter in encode_message. Defaults to None.	None

Source code in a2gmeasurements.py

def socket_send_cmd(self, type_cmd=None, data=None):
    """
    Creates a message by the name of the request/command. Wrapper to ``encode_message``.

    Args:
        type_cmd (int, optional): refers to the ``cmd`` parameter in ``encode_message``. Defaults to None.
        data (int, optional): refers to the ``data`` parameter in ``encode_message``. Defaults to None.
    """

    if type_cmd == 'SETGIMBAL':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x03, data=data)
    elif type_cmd == 'FOLLOWGIMBAL':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x01, data=data)
    elif type_cmd == 'GETGPS':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x02)
    elif type_cmd == 'CLOSEDGUI':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x07)
    elif type_cmd == 'STARTDRONERFSOC':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x04, data=data)
    elif type_cmd == 'STOPDRONERFSOC':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x05)
    elif type_cmd == 'FINISHDRONERFSOC':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x06)
    elif type_cmd == 'SETIRF':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x02, cmd=0x01, data=data)
    elif type_cmd == 'SETREMOTEFMFLAG':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x08, data=data)
    elif type_cmd == 'SETREMOTESTOPFM':
        frame = self.encode_message(source_id=0x01, destination_id=0x02, message_type=0x01, cmd=0x09)
    if self.ID == 'DRONE':
        self.socket.sendall(frame)
    elif self.ID == 'GROUND':
        self.a2g_conn.sendall(frame)
    print(f"[DEBUG]: This {self.ID} node sends {type_cmd} cmd")