Gremsy H16 gimbal

Python Class that works as the driver for the gimbal Gremsy H16.

The gimbal should be connected to the host computer through an TTL2USB connection (check "Manual A2GMeasurements").

It creates a thread (called here a gimbal thread) to handle the communication between the gimbal and this host computer.

Gimbal's rotational speed (both in yaw -azimuth-, and pitch -elevation-) depends on a value between [-100, 100] as this is the natural range of values in most Remote Controllers (i.e. FrSky X8R).

Gmbal's rotational speed dependence on the RC controlling interval is neither linear, nor symmetrical: 1. Non-linear: a change from 10 to 20 is not equivalent to a change from 20 to 30 2. Non-symmetrical: a change from 10 to 20 is not equivalent to a change from -10 to -20. A change from 10 to 20 (or -10 to -20) in yaw is not equivalent to a change from 10 to 20 (or -10 to -20) in pitch.

Gimbal's angle (either yaw or pitch) depends on: 1) the RC controlling interval and 2) the time the given RC control value is hold. This dependence is measured as described in "Manual A2GMeasurements".

Gimbal's angle can only be controlled by using the RC control value and the time the serial SBUS will hold that control value.

This class relies on heavily on the SBUSEncoder class, as that is the class decoding the sbus protocol from Gremsy.

This class is meant to be equivalent to GimbalRS2. Mainly by implementing the setPosControl for this gimbal.

Source code in a2gmeasurements.py

class GimbalGremsyH16:
    """
    Python Class that works as the driver for the gimbal Gremsy H16.

    The gimbal should be connected to the host computer through an TTL2USB connection (check "Manual A2GMeasurements"). 

    It creates a thread (called here a gimbal thread) to handle the communication between the gimbal and this host computer.

    Gimbal's rotational speed (both in yaw -azimuth-, and pitch -elevation-) depends on a value between [-100, 100] as this is the natural range of values in most Remote Controllers (i.e. FrSky X8R).

    Gmbal's rotational speed dependence on the RC controlling interval is neither linear, nor symmetrical:
     1. Non-linear:  a change from 10 to 20 is not equivalent to a change from 20 to 30
     2. Non-symmetrical: a change from 10 to 20 is not equivalent to a change from -10 to -20. A change from 10 to 20 (or -10 to -20) in yaw is not equivalent to a change from 10 to 20 (or -10 to -20) in pitch.

    Gimbal's angle (either yaw or pitch) depends on: 1) the RC controlling interval and 2) the time the given RC control value is hold. This dependence is measured as described in "Manual A2GMeasurements".

    Gimbal's angle can only be controlled by using the *RC control value* and the *time* the serial SBUS will hold that control value.

    This class relies on heavily on the ``SBUSEncoder`` class, as that is the class decoding the sbus protocol from Gremsy. 

    This class is meant to be equivalent to ``GimbalRS2``. Mainly by implementing the ``setPosControl`` for this gimbal.
    """

    def __init__(self, speed_time_azimuth_table=None, speed_time_elevation_table=None):
        """
        Constructor for the class. 

        Loads the measured angle (yaw and pitch, each one separately) dependence on RC control value and time. Replace ``load_measured_data_august_2023`` or reimplement it if newer/better measurements of this dependence are available. 

        Args:
            speed_time_azimuth_table (numpy.ndarray, optional): array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured azimuth (yaw). Defaults to None.
            speed_time_elevation_table (numpy.ndarray, optional): array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured elevation (pitch). Defaults to None.
        """

        self.cnt_imu_readings = 0

        if speed_time_azimuth_table is not None:
            self.speed_time_azimuth_table = speed_time_azimuth_table
            self.speed_time_elevation_table = speed_time_elevation_table
        else:
            #self.load_measured_data_july_2023()
            self.load_measured_data_august_2023()

        # Assume linear fit:
        # For a longer range of speeds than the used in the default data, it is very likely that the fit won't be linear
        self.fit_model_to_gimbal_angular_data(model='gp')    
        self.start_imu_thread()

    def define_home_position(self):
        """
        Defines the center of the coordinated system (yaw, pitch). To do so, it checks if the gimbal is moving (in both azimuth and elevation) and if not, read the angles provided by the IMU and set them as the home position. Due to this operation, this function **must** be called in ``__init__`` or before any ``setPosControl`` call.

        As the gimbal controller from the manufacturer can't be accesed, imu readings are not available. An external IMU is required and any cheap raspberry pi pico is capable of providing decent IMU support. Potential magnetic interferences between the IMU readings of the raspberry pi pico and the motors of the gimbal have not been researched.

        **NOTE**: this function *requires to be further tested*, if the Gremsy gimbal is to be used again as part of the channel sounder system. The reason is that when checking if the gimbal is moving, a good tolerated error (tol_err = abs(angle_now - angle_before)) must be set.
        """
        print("[DEBUG]: Defining HOME for Gremsy... This might take a second")

        start_time = time.time()
        yaw_before = 1000 # Set it high for first iteration
        pitch_before = 1000 # Set it high for first iteration
        cnt = self.cnt_imu_readings
        DONT_STOP = True
        tol_err = 2
        while(DONT_STOP):
            if cnt != self.cnt_imu_readings:
                yaw = self.last_imu_reading['YAW']
                pitch = self.last_imu_reading['PITCH']

                if np.abs(yaw - yaw_before) <= tol_err:
                    CONDITION_YAW_SATISFIED = True
                else:
                    CONDITION_YAW_SATISFIED = False

                if np.abs(pitch - pitch_before) <= tol_err:
                    CONDITION_PITCH_SATISFIED = True
                else:
                    CONDITION_PITCH_SATISFIED = False

                if CONDITION_YAW_SATISFIED and CONDITION_PITCH_SATISFIED:
                    break        

                yaw_before = yaw
                pitch_before = pitch
                cnt = self.cnt_imu_readings

        self.home_position = {'YAW': yaw, 'PITCH': pitch}
        print(f"TIME SPENT DEFINE HOME POSITION: {time.time() - start_time}")

    def start_imu_thread(self, COM_PORT='COM21'):
        """
        Connects to the IMU and creates a new thread (the imu thread) to read the angle data from the IMU.

        :param COM_PORT: , defaults to 'COM21'
        :type COM_PORT: str, optional

        Args:
            COM_PORT (str, optional): port where the IMU is connected. If this host computer's OS is Windows it would be ``COM#``, if it is Linux it would be ``/dev/ttyUSB#``. Defaults to ``COM21``.
        """

        try:
            self.imu_serial = serial.Serial(COM_PORT, 9600)
            print("[DEBUG]: Connected to IMU")
        except Exception as e:
            print("[DEBUG]: Exception when connecting to IMU: ", e)
        else:
            try:
                self.event_stop_thread_imu = threading.Event()                
                self.thread_read_imu = threading.Thread(target=self.receive_imu_data, args=(self.event_stop_thread_imu,))
                self.thread_read_imu.start()
                print(f"[DEBUG]: READ IMU THREAD STARTED")
            except Exception as e:
                print("[DEBUG]: Exception when starting READ IMU thread")
        #for (this_port, desc, verbose) in sorted(comports()):
        #    print(f"PORT: {this_port}, DESC: {desc}, verbose: {verbose}")

    def receive_imu_data(self, stop_event):
        """
        Callback function for the imu thread. Read the yaw, pitch, roll angles and stores them in the attribute ``last_imu_reading`` of this class.

        Args:
            stop_event (threading.Event): when this is set, this function won't do anything.
        """

        while not stop_event.is_set():
            data = self.imu_serial.readline().decode('utf-8').strip()
            data = data.split(',')

            self.last_imu_reading = {'YAW': float(data[0]), 'PITCH': float(data[1]), 'ROLL': float(data[2])}
            self.cnt_imu_readings = self.cnt_imu_readings + 1

            if self.cnt_imu_readings > 1e12:
                self.cnt_imu_readings = 0

        #print(f"YAW: {data[0]}, PITCH: {data[1]}, ROLL: {data[2]}")

    def stop_thread_imu(self):
        """
        Stops the imu thread and closed the serial port where the IMU is connected.
        """

        if self.thread_read_imu.is_alive():
            self.event_stop_thread_imu.set()

        self.imu_serial.close()

    def fit_model_to_gimbal_angular_data(self, model='linear'):
        """
        Fits a model to the measured angle (yaw, pitch) dependence on time and speed (RC control value). There are two models: a linear model and a gaussian regressor.

        The linear model is suitable specific range of "speeds" and time, since the non-linear dependence can be linearized. However, this *range must be defined*.

        For the gaussian regressor, *more training samples (RC control value, time, angle) over the full range are required* to avoid overfitting and bad predicting behaviour.

        There is either a linear or gp model for the RC control positive values and another one for the RC control negative values.

        Args:
            model (str, optional): either ``linear`` or ``gp``. Defaults to ``linear``.
        """

        # Define the kernel for Gaussian Process Regressor
        kernel = 1.0 * RBF(length_scale=1.0, length_scale_bounds=(1e-2, 1e2)) + WhiteKernel(noise_level=1e-5, noise_level_bounds=(1e-10, 1e-3))

        is_positive_azimuth = self.speed_time_azimuth_table > 0
        is_positive_elevation = self.speed_time_elevation_table > 0

        X = self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 0:2]
        y = np.rad2deg(self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 2])
        if model =='linear':
            self.az_speed_pos_regresor = LinearRegression().fit(X, y)
        elif model == 'gp':
            self.az_speed_pos_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y)
        self.score_az_speed_pos_regresor = self.az_speed_pos_regresor.score(X, y)
        print("[DEBUG]: POSITIVE SPEEDS (LEFT), AZIMUTH, R^2 Score Linear Reg: ", self.score_az_speed_pos_regresor)

        X = self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 0:2]
        y = np.rad2deg(self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 2])
        if model == 'linear':
            self.az_speed_neg_regresor = LinearRegression().fit(X, y)
        elif model == 'gp':
            self.az_speed_neg_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y)
        self.score_az_speed_neg_regresor = self.az_speed_neg_regresor.score(X, y)
        print("[DEBUG]: NEGATIVE SPEEDS (RIGHT), AZIMUTH, R^2 Score Linear Reg: ", self.score_az_speed_neg_regresor)

        X = self.speed_time_elevation_table[~is_positive_elevation[:, 0], 0:2]
        y = np.rad2deg(self.speed_time_elevation_table[~is_positive_elevation[:, 0], 2])
        if model == 'linear':
            self.el_speed_neg_regresor = LinearRegression().fit(X, y)
        elif model == 'gp':
            self.el_speed_neg_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y).fit(X, y)
        self.score_el_speed_neg_regresor = self.el_speed_neg_regresor.score(X, y)
        print("[DEBUG]: NEGATIVE SPEEDS (DOWN), ELEVATION, R^2 Score Linear Reg: ", self.score_el_speed_neg_regresor)

        X = self.speed_time_elevation_table[is_positive_elevation[:, 0], 0:2]
        y = np.rad2deg(self.speed_time_elevation_table[is_positive_elevation[:, 0], 2])
        if model == 'linear':
            self.el_speed_pos_regresor = LinearRegression().fit(X, y)
        elif model == 'gp':
            self.el_speed_pos_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y).fit(X, y)
        self.score_el_speed_pos_regresor = self.el_speed_pos_regresor.score(X, y)
        print("[DEBUG]: POSITIVE SPEEDS (UP), ELEVATION, R^2 Score Linear Reg: ", self.score_el_speed_neg_regresor)

    def setPosControlGPModel(self, yaw=0, pitch=0):
        """
        Finds the RC control value and time giving the desired yaw (or pitch, or both separately) for the gaussian regressor. 

        Uses an iterative approach to find the RC control value and time to hold it by smartly searching in the grid composed by the RC control values and the times to hold it. The initial value of for the "speed" (RC control value) influences the convergence of the iterative method.

        NOTE FOR DEVELOPERS: *this function requires*:
         1. a better grid of measured "speed", time, angle. With the actual training samples, the prediction does not give physical consistent results (i.e. the time it takes to move 60 degrees at speed 20 is smaller than the one it takes to move 60 degrees at a lower speed). This is because the grid is coarse and not equally sampled.
         2. this function should guarantee that the returned time is always positive. Negative times does not make physical sense. Furthermore, it should also guarantee that the time is above a certain threshold (i.e. 2 seconds), during which the gimbal will accelerate until reaching the desired speed. In a realistic gimbal, the acceleration of the gimbal is not infinite. On the contrary, gimbal's speed vs time dependence follows usually a trapezoidal curve, which means that there is some time required (the threshold) for the gimbal to reach the plateau of the trapezoid (desired speed).
         3. the caller of this function (``setPosControl``) to handle when an exeception is raised.

        Args:
            yaw (int, optional): desired yaw angle to set in the interval [-180,180]. Defaults to 0.
            pitch (int, optional): desired pitch angle to set. Defaults to 0.

        Raises:
            Exception: when the optimization local function ``find_feature_values_for_angle`` does not converge.

        Returns:
            speed_yaw (float): required RC control value to set yaw.
            time_yaw (float): required time to set yaw. 
            speed_pitch (float): required RC control value to set pitch
            time_pitch (float): required time to set pitch.
        """

        start_time = time.time()

        # Define a function to find the corresponding X values for the desired Y
        def find_feature_values_for_angle(model, desired_angle, suggested_speed):           
            # Define a function to minimize (difference between predicted and desired Y)
            def objective_function(x):
                #x = np.atleast_2d(x)
                y = np.array([[suggested_speed, x]])
                return np.abs(model.predict(y) - desired_angle)

            # Initialize with a guess for speed and time
            initial_guess = 3

            # Use an optimization method to find the feature values that result in the desired yaw angle
            result = minimize_scalar(objective_function, bounds=(0, 50), method='bounded')
            #result = minimize(objective_function, initial_guess, method='Nelder-Mead')

            if result.success:
                return result.x
            else:
                raise Exception("Optimization did not converge.")

        total_time = time.time() - start_time

        # Find the corresponding feature values (speed and time) for the desired angle
        if yaw > 0.0 and yaw < 10:
            speed_yaw = 10.0
            time_yaw = find_feature_values_for_angle(self.az_speed_pos_regresor, yaw, speed_yaw)
        elif yaw >= 10.0 and yaw < 60:
            speed_yaw = 13.0
            time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
        elif yaw >= 60:
            speed_yaw = 15.0
            time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
        if yaw < 0.0 and yaw > -10:
            speed_yaw = -3.0
            time_yaw = find_feature_values_for_angle(self.az_speed_pos_regresor, yaw, speed_yaw)
        elif yaw <= -10.0 and yaw > -60:
            speed_yaw = -7.0
            time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
        elif yaw <= -60:
            speed_yaw = -10.0
            time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
        elif yaw == 0.0:
            speed_yaw = 0
            time_yaw = 0
        if pitch > 0.0 and pitch < 10.0:
            speed_pitch = 5.0
            time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
        elif pitch >=10 and pitch < 60:
            speed_pitch = 8.0
            time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
        elif pitch >= 60:
            speed_pitch = 10.0
            time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
        elif pitch < 0.0 and pitch > -10:
            speed_pitch = -5.0
            time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
        elif pitch <-10 and pitch > -60:
            speed_pitch = -8.0
            time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
        elif pitch <= -60:
            speed_pitch = -10.0
            time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
        elif pitch == 0.0:
            speed_pitch = 0
            time_pitch = 0

        #print(f"[DEBUG]: Time it took to find the speed and time corresponding to the DESIRED ANGLE: {total_time}")

        return speed_yaw, time_yaw, speed_pitch, time_pitch

    def load_measured_drifts(self):
        """
        Loads measured drift angles from the experiment described on "Manual A2GMeasurements".        
        """ 
        drift_with_low_speed_counter = [[137, self.gremsy_angle(2, 1.97, 0.10)],
                                        [144, self.gremsy_angle(1.97, 1.94, 0.10)],
                                        [145, self.gremsy_angle(1.94, 1.927, 0.10)],
                                        [156, self.gremsy_angle(1.927, 1.911, 0.10)],
                                        [164, self.gremsy_angle(1.911, 1.898, 0.10)],
                                        [148, self.gremsy_angle(1.898, 1.892, 0.10)],
                                        [164, self.gremsy_angle(1.892, 1.89, 0.10)],
                                        [176, self.gremsy_angle(1.89, 1.894, 0.10)],
                                        [185, self.gremsy_angle(1.894, 1.9, 0.10)],
                                        [159, self.gremsy_angle(1.9, 1.914, 0.10)],
                                        [159, self.gremsy_angle(1.914, 1.932, 0.10)],
                                        [146, self.gremsy_angle(1.932, 1.954, 0.10)]]

        drift_without_low_speed_counter = [[28.91, self.gremsy_angle(2, 1.971, 0.10)],
                                           [28.43, self.gremsy_angle(1.971, 1.95, 0.10)],
                                           [28.46, self.gremsy_angle(1.95, 1.923, 0.10)],
                                           [30.14, self.gremsy_angle(1.923, 1.9, 0.10)],
                                           [29.76, self.gremsy_angle(1.9, 1.888, 0.10)],
                                           [29.36, self.gremsy_angle(1.888, 1.884, 0.10)],
                                           [31.41, self.gremsy_angle(1.884, 1.872, 0.10)],
                                           [31.3, self.gremsy_angle(1.872, 1.881, 0.10)],
                                           [28.77, self.gremsy_angle(1.881, 1.89, 0.10)],
                                           [31.56, self.gremsy_angle(1.89, 1.912, 0.10)],
                                           [29.15, self.gremsy_angle(1.912, 1.935, 0.10)]]

        self.drift_with_low_speed_counter = drift_with_low_speed_counter
        self.drift_without_low_speed_counter = drift_without_low_speed_counter
        self.avg_drift_with_low_speed_counter = np.array(drift_with_low_speed_counter).mean(axis=0) # 2D array 
        self.avg_drift_without_low_speed_counter = np.array(drift_without_low_speed_counter).mean(axis=0) # 2D array

    def load_measured_data_july_2023(self):
        """
        Loads a set of measured data extracted from the experiment described on "Manual A2GMeasurements". 

        This table contains as columns the speed [-100, 100], time [s], and the azimuth angle computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

        NOTE FOR DEVELOPERS: *the experiment described in* "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.
        """        
        speed_time_azimuth_table = [[15, 6, self.gremsy_angle(1.903, 1.949, 0.87)], 
                        [15, 7, self.gremsy_angle(1.955, 1.926, 1)],
                        [15, 8, self.gremsy_angle(2.071, 1.897, 1.19)],
                        [15, 9, self.gremsy_angle(2.023, 1.949, 1.315)],
                        [16, 5, self.gremsy_angle(1.879, 2.078, 0.875)],
                        [16, 6, self.gremsy_angle(1.883, 2.069, 1.025)],
                        [16, 7, self.gremsy_angle(1.897, 2.219, 1.26)],
                        [14, 7, self.gremsy_angle(1.886, 1.994, 0.86)],
                        [14, 8, self.gremsy_angle(1.881, 2.069, 1)],
                        [14, 9, self.gremsy_angle(1.888, 2.086, 1.134)],
                        [-14, 4, self.gremsy_angle(1.922, 2.047, 1.255)],
                        [-14, 5, self.gremsy_angle(1.961, 2.117, 1.59)],
                        [-14, 6, self.gremsy_angle(2.106, 2.089, 1.93)],
                        [-13, 4, self.gremsy_angle(2.034, 1.909, 1.165)],
                        [-13, 5, self.gremsy_angle(2.025, 1.985, 1.44)],
                        [-13, 6, self.gremsy_angle(2.183, 1.98, 1.79)]]
        self.speed_time_azimuth_table = np.array(speed_time_azimuth_table)

        speed_time_elevation_table = [[-5, 1, self.gremsy_angle(1.884, 1.882, 0.1)],
                                            [-5, 2, self.gremsy_angle(1.881, 1.889, 0.175)],
                                            [-5, 3, self.gremsy_angle(1.89, 1.934, 0.272)],
                                            [-5, 4, self.gremsy_angle(1.889, 1.891, 0.345)]]
        self.speed_time_elevation_table = np.array(speed_time_elevation_table)        

    def load_measured_data_august_2023(self):
        """
        Loads a second set of measured data extracted from the experiment described on "Manual A2GMeasurements".

        This table contains as columns the speed [-100, 100], time [s], and the angle (azimuth, elevation) computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

        NOTE FOR DEVELOPERS: *the experiment described in* "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.
        """        
        speed_time_azimuth_table = [[15, 6, self.gremsy_angle(1.903, 1.949, 0.87)], 
                        [15, 7, self.gremsy_angle(1.955, 1.926, 1)],
                        [15, 8, self.gremsy_angle(2.071, 1.897, 1.19)],
                        [15, 9, self.gremsy_angle(2.023, 1.949, 1.315)],
                        [16, 5, self.gremsy_angle(1.879, 2.078, 0.875)],
                        [16, 6, self.gremsy_angle(1.883, 2.069, 1.025)],
                        [16, 7, self.gremsy_angle(1.897, 2.219, 1.26)],
                        [14, 7, self.gremsy_angle(1.886, 1.994, 0.86)],
                        [14, 8, self.gremsy_angle(1.881, 2.069, 1)],
                        [14, 9, self.gremsy_angle(1.888, 2.086, 1.134)],
                        [-14, 4, self.gremsy_angle(1.922, 2.047, 1.255)],
                        [-14, 5, self.gremsy_angle(1.961, 2.117, 1.59)],
                        [-14, 6, self.gremsy_angle(2.106, 2.089, 1.93)],
                        [-13, 4, self.gremsy_angle(2.034, 1.909, 1.165)],
                        [-13, 5, self.gremsy_angle(2.025, 1.985, 1.44)],
                        [-13, 6, self.gremsy_angle(2.183, 1.98, 1.79)],
                        [-15, 4, self.gremsy_angle(2.38, 1.574, 1.666)],
                        [-15, 5, self.gremsy_angle(2.183, 1.538, 1.836)],
                        [-15, 6, self.gremsy_angle(2.183, 1.546, 2.111)],
                        [-16, 4, self.gremsy_angle(2.183, 1.558, 1.648)],
                        [-16, 5, self.gremsy_angle(2.183, 1.537, 1.934)],
                        [-16, 6, self.gremsy_angle(2.183, 1.562, 2.215)],
                        [-17, 4, self.gremsy_angle(2.183, 1.55, 1.71)],
                        [-17, 5, self.gremsy_angle(2.183, 1.54, 2.103)],
                        [-17, 6, self.gremsy_angle(2.183, 1.586, 2.327)],
                        [-18, 4, self.gremsy_angle(2.183, 1.541, 1.799)],
                        [-18, 5, self.gremsy_angle(2.183, 1.623, 2.109)],
                        [-18, 6, self.gremsy_angle(2.183, 1.623, 2.463)],
                        [-19, 4, self.gremsy_angle(2.183, 1.537, 1.885)],
                        [-19, 5, self.gremsy_angle(2.183, 1.563, 2.215)],
                        [-19, 6, self.gremsy_angle(2.183, 1.663, 2.582)],
                        [-20, 4, self.gremsy_angle(2.183, 1.537, 1.952)],
                        [-20, 5, self.gremsy_angle(2.183, 1.585, 2.321)],
                        [-20, 6, self.gremsy_angle(2.183, 1.718, 2.72)]]
        self.speed_time_azimuth_table = np.array(speed_time_azimuth_table)

        speed_time_elevation_table = [[10, 3, self.gremsy_angle(1.526, 1.529, 0.07)],
                                        [10, 4, self.gremsy_angle(1.526, 1.531, 0.096)],
                                        [10, 5, self.gremsy_angle(1.526, 1.532, 0.11)],
                                        [10, 6, self.gremsy_angle(1.526, 1.533, 0.122)],
                                        [10, 7, self.gremsy_angle(1.526, 1.537, 0.16)],
                                        [10, 8, self.gremsy_angle(1.526, 1.540, 0.183)],
                                        [11, 3, self.gremsy_angle(1.526, 1.543, 0.124)],
                                        [11, 4, self.gremsy_angle(1.526, 1.548, 0.163)],
                                        [11, 5, self.gremsy_angle(1.526, 1.542, 0.199)],
                                        [11, 6, self.gremsy_angle(1.526, 1.554, 0.239)],
                                        [11, 7, self.gremsy_angle(1.526, 1.548, 0.279)],
                                        [12, 3, self.gremsy_angle(1.526, 1.538, 0.168)],
                                        [12, 4, self.gremsy_angle(1.526, 1.545, 0.227)],
                                        [12, 5, self.gremsy_angle(1.526, 1.555, 0.283)],
                                        [12, 6, self.gremsy_angle(1.526, 1.568, 0.343)],
                                        [12, 7, self.gremsy_angle(1.526, 1.580, 0.4)],
                                        [13, 3, self.gremsy_angle(1.526, 1.548, 0.244)],
                                        [13, 4, self.gremsy_angle(1.526, 1.56, 0.311)],
                                        [13, 5, self.gremsy_angle(1.526, 1.578, 0.395)],
                                        [13, 6, self.gremsy_angle(1.526, 1.598, 0.448)],
                                        [13, 7, self.gremsy_angle(1.526, 1.656, 0.64)],
                                        [14, 2, self.gremsy_angle(1.526, 1.542, 0.184)],
                                        [14, 3, self.gremsy_angle(1.526, 1.558, 0.286)],
                                        [14, 4, self.gremsy_angle(1.526, 1.578, 0.378)],
                                        [14, 5, self.gremsy_angle(1.526, 1.603, 0.474)],
                                        [14, 6, self.gremsy_angle(1.526, 1.635, 0.574)],
                                        [15, 1, self.gremsy_angle(1.526, 1.531, 0.11)],
                                        [15, 2, self.gremsy_angle(1.526, 1.542, 0.209)],
                                        [15, 3, self.gremsy_angle(1.526, 1.565, 0.334)],
                                        [15, 4, self.gremsy_angle(1.526, 1.594, 0.448)],
                                        [15, 5, self.gremsy_angle(1.526, 1.63, 0.567)],
                                        [16, 1, self.gremsy_angle(1.526, 1.533, 0.125)],
                                        [16, 2, self.gremsy_angle(1.526, 1.549, 0.253)],
                                        [16, 3, self.gremsy_angle(1.526, 1.575, 0.378)],
                                        [16, 4, self.gremsy_angle(1.526, 1.614, 0.519)],
                                        [16, 5, self.gremsy_angle(1.526, 1.666, 0.665)],
                                        [17, 1, self.gremsy_angle(1.526, 1.535, 0.147)],
                                        [17, 2, self.gremsy_angle(1.526, 1.557, 0.291)],
                                        [17, 3, self.gremsy_angle(1.526, 1.59, 0.435)],
                                        [17, 4, self.gremsy_angle(1.526, 1.642, 0.6)],
                                        [17, 5, self.gremsy_angle(1.526, 1.708, 0.766)],
                                        [18, 1, self.gremsy_angle(1.526, 1.537, 0.164)],
                                        [18, 2, self.gremsy_angle(1.526, 1.563, 0.328)],
                                        [18, 3, self.gremsy_angle(1.526, 1.608, 0.502)],
                                        [18, 4, self.gremsy_angle(1.526, 1.676, 0.693)],
                                        [18, 5, self.gremsy_angle(1.526, 1.761, 0.884)],
                                        [19, 0.5, self.gremsy_angle(1.526, 1.529, 0.088)],
                                        [19, 1, self.gremsy_angle(1.526, 1.538, 0.176)],
                                        [19, 2, self.gremsy_angle(1.526, 1.571, 0.363)],
                                        [19, 3, self.gremsy_angle(1.526, 1.626, 0.558)],
                                        [19, 4, self.gremsy_angle(1.526, 1.707, 0.766)],
                                        [20, 0.5, self.gremsy_angle(1.526, 1.53, 0.1)],
                                        [20, 1, self.gremsy_angle(1.526, 1.541, 0.2)],
                                        [20, 2, self.gremsy_angle(1.526, 1.579, 0.395)],
                                        [20, 3, self.gremsy_angle(1.526, 1.648, 0.616)],
                                        [20, 4, self.gremsy_angle(1.526, 1.742, 0.845)],
                                        [-5, 3, self.gremsy_angle(1.769, 1.652, 0.268)],
                                        [-5, 6, self.gremsy_angle(1.769, 1.58, 0.5)],
                                        [-5, 7, self.gremsy_angle(1.769, 1.562, 0.575)],
                                        [-5, 8, self.gremsy_angle(1.769, 1.548, 0.648)],
                                        [-5, 10, self.gremsy_angle(1.769, 1.534, 0.758)],
                                        [-6, 3, self.gremsy_angle(1.769, 1.632, 0.324)],
                                        [-6, 6, self.gremsy_angle(1.769, 1.556, 0.608)],
                                        [-6, 8, self.gremsy_angle(1.769, 1.531, 0.785)],
                                        [-6, 10, self.gremsy_angle(1.769, 1.527, 0.958)],
                                        [-7, 3, self.gremsy_angle(1.769, 1.713, 0.385)],
                                        [-7, 6, self.gremsy_angle(1.769, 1.534, 0.717)],
                                        [-7, 8, self.gremsy_angle(1.769, 1.526, 0.925)],
                                        [-7, 10, self.gremsy_angle(1.769, 1.541, 1.135)],
                                        [-8, 3, self.gremsy_angle(1.769, 1.599, 0.433)],
                                        [-8, 6, self.gremsy_angle(1.769, 1.53, 0.983)],
                                        [-8, 8, self.gremsy_angle(1.769, 1.533, 1.058)],
                                        [-8, 10, self.gremsy_angle(1.769, 1.574, 1.309)],
                                        [-9, 3, self.gremsy_angle(1.769, 1.581, 0.496)],
                                        [-9, 6, self.gremsy_angle(1.769, 1.527, 0.934)],
                                        [-9, 8, self.gremsy_angle(1.769, 1.554, 1.214)],
                                        [-9, 9, self.gremsy_angle(1.769, 1.588, 1.364)],
                                        [-10, 2, self.gremsy_angle(1.769, 1.612, 0.387)],
                                        [-10, 5, self.gremsy_angle(1.769, 1.527, 0.883)],
                                        [-10, 7, self.gremsy_angle(1.769, 1.551, 1.197)],
                                        [-10, 8, self.gremsy_angle(1.769, 1.588, 1.364)],
                                        [-11, 2, self.gremsy_angle(1.769, 1.601, 0.42)],
                                        [-11, 5, self.gremsy_angle(1.769, 1.527, 0.96)],
                                        [-11, 6, self.gremsy_angle(1.769, 1.541, 1.136)],
                                        [-11, 7, self.gremsy_angle(1.769, 1.577, 1.322)],
                                        [-12, 2, self.gremsy_angle(1.769, 1.588, 0.467)],
                                        [-12, 5, self.gremsy_angle(1.769, 1.532, 1.048)],
                                        [-12, 6, self.gremsy_angle(1.769, 1.559, 1.247)],
                                        [-12, 7, self.gremsy_angle(1.769, 1.611, 1.446)],
                                        [-13, 2, self.gremsy_angle(1.769, 1.579, 0.502)],
                                        [-13, 5, self.gremsy_angle(1.769, 1.542, 1.144)],
                                        [-13, 6, self.gremsy_angle(1.769, 1.588, 1.363)],
                                        [-13, 7, self.gremsy_angle(1.769, 1.672, 1.611)],
                                        [-14, 2, self.gremsy_angle(1.769, 1.572, 0.534)],
                                        [-14, 5, self.gremsy_angle(1.769, 1.558, 1.231)],
                                        [-14, 6, self.gremsy_angle(1.769, 1.624, 1.483)],
                                        [-14, 7, self.gremsy_angle(1.769, 1.672, 1.752)],
                                        [-15, 2, self.gremsy_angle(1.769, 1.565, 0.559)],
                                        [-15, 4, self.gremsy_angle(1.769, 1.534, 1.069)],
                                        [-15, 5, self.gremsy_angle(1.769, 1.58, 1.328)],
                                        [-15, 6, self.gremsy_angle(1.769, 1.666, 1.6)],
                                        [-16, 2, self.gremsy_angle(1.769, 1.558, 0.601)],
                                        [-16, 4, self.gremsy_angle(1.769, 1.541, 1.136)],
                                        [-16, 5, self.gremsy_angle(1.769, 1.604, 1.42)],
                                        [-16, 6, self.gremsy_angle(1.769, 1.722, 1.73)],
                                        [-17, 2, self.gremsy_angle(1.769, 1.55, 0.641)],
                                        [-17, 4, self.gremsy_angle(1.769, 1.554, 1.212)],
                                        [-17, 5, self.gremsy_angle(1.769, 1.634, 1.512)],
                                        [-17, 6, self.gremsy_angle(1.769, 1.74, 1.771)],
                                        [-18, 1, self.gremsy_angle(1.769, 1.623, 0.355)],
                                        [-18, 3, self.gremsy_angle(1.769, 1.528, 0.98)],
                                        [-18, 4, self.gremsy_angle(1.769, 1.568, 1.284)],
                                        [-18, 5, self.gremsy_angle(1.769, 1.677, 1.625)],
                                        [-19, 1, self.gremsy_angle(1.769, 1.615, 0.38)],
                                        [-19, 3, self.gremsy_angle(1.769, 1.531, 1.031)],
                                        [-19, 4, self.gremsy_angle(1.769, 1.591, 1.374)],
                                        [-19, 5, self.gremsy_angle(1.769, 1.726, 1.774)],
                                        [-20, 1, self.gremsy_angle(1.769, 1.609, 0.401)],
                                        [-20, 3, self.gremsy_angle(1.769, 1.536, 1.086)],
                                        [-20, 4, self.gremsy_angle(1.769, 1.613, 1.451)],
                                        [-20, 5, self.gremsy_angle(1.769, 1.737, 1.764)]]
        self.speed_time_elevation_table = np.array(speed_time_elevation_table)

    def gremsy_angle(self, a_i, a_ip, b_i):
        """
        Computes the angle between the sides of the triangle given by a_i and  a_i+1. The opposite side to the angle computed is the one defined by b_i. 

        A definition of this distances can be found in "Manual A2GMeasurements".

        NOTE FOR DEVELOPERS: *the experiment described in * "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.

        Args:
            a_i (float): defined in "Manual A2GMeasurements".
            a_ip (float): defined in "Manual A2GMeasurements".
            b_i (float): defined in "Manual A2GMeasurements".

        Returns:
            Radians (float): mentioned angle in radians.
        """

        tmp = (a_i**2 + a_ip**2 - b_i**2)/(2*a_i*a_ip)
        return np.arccos(tmp)

    def plot_linear_reg_on_near_domain(self, loaded='august'):
        """
        Generates a figure with 3 subplots with measured values (default measured values or new values measured for more (speed, time) tuples given as a parameter to the class) and with linear regression model applied to the measured values.

        1. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> azimuth 
        2. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> azimuth 
        3. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> elevation

        If ``loaded`` is august then the figure has 4 subplots, with the 4th being
        4. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> elevation

        Args:
            loaded (str, optional): which table loaded: the one from ``load_measured_data_july_2023`` or the one from ``load_measured_data_august_2023``. Defaults to ``august``.
        """

        is_positive_azimuth = self.speed_time_azimuth_table > 0
        is_positive_elevation = self.speed_time_elevation_table > 0       

        # We are going to use multiple if - else statements of the same kind just to maintain some readibility of the code
        if loaded == 'august':
            fig = make_subplots(rows=2, cols=2, specs=[[{"type": "scene"}, {"type": "scene"}], [{"type": "scene"}, {"type": "scene"}]],
                                subplot_titles=("Pos. Speed - Azimuth", "Neg. Speed - Azimuth", "Neg. Speed - Elevation", "Pos. Speed - Elevation"))
        else:
            fig = make_subplots(rows=1, cols=3, specs=[[{"type": "scene"}, {"type": "scene"}, {"type": "scene"}]],
                                subplot_titles=("Pos. Speed - Azimuth", "Neg. Speed - Azimuth", "Neg. Speed - Elevation"))

        X, Y = np.meshgrid(np.linspace(10, 20, num=11), np.linspace(5, 9, num=5))
        XY = np.c_[X.flatten(), Y.flatten()]
        Z = self.az_speed_pos_regresor.predict(XY)        

        fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=1, col=1)
        fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 0], y=self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 1],
                z=np.rad2deg(self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=1, col=1)

        X, Y = np.meshgrid(np.linspace(-20, -10, num=11), np.linspace(4, 6, num=3))    
        XY = np.c_[X.flatten(), Y.flatten()]
        Z = self.az_speed_neg_regresor.predict(XY)

        fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=1, col=2)
        fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 0], y=self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 1],
                z=np.rad2deg(self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=1, col=2)

        if loaded == 'august':
            X, Y = np.meshgrid(np.linspace(-20, -5, num=16), np.linspace(1, 10, num=10))
            nrow = 2
            ncol = 1
        else:
            X, Y = np.meshgrid(np.linspace(-10, -5, num=6), np.linspace(1, 4, num=4))
            nrow = 1
            ncol = 3
        XY = np.c_[X.flatten(), Y.flatten()]
        Z = self.el_speed_neg_regresor.predict(XY)

        fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=nrow, col=ncol)
        fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_elevation_table[~is_positive_elevation[:, 0], 0], y=self.speed_time_elevation_table[~is_positive_elevation[:, 0], 1],
                z=np.rad2deg(self.speed_time_elevation_table[~is_positive_elevation[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=nrow, col=ncol)

        if loaded == 'august':
            X, Y = np.meshgrid(np.linspace(10, 20, num=11), np.linspace(0, 9, num=10))
            XY = np.c_[X.flatten(), Y.flatten()]
            Z = self.el_speed_pos_regresor.predict(XY)

            fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=2, col=2)
            fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_elevation_table[is_positive_elevation[:, 0], 0], y=self.speed_time_elevation_table[is_positive_elevation[:, 0], 1],
                    z=np.rad2deg(self.speed_time_elevation_table[is_positive_elevation[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=2, col=2)

        fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                        yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                        zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                        row=1, col=1,)
        fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                        yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                        zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                        row=1, col=2,)
        fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                        yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                        zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                        row=1, col=3,)

        fig.update_layout(autosize=False, width=1400, height=800, margin=dict(l=50, r=50, t=50, b=50),)
        fig.show()

    def start_thread_gimbal(self):
        """
        Creates an instance of the ``SBUSEncoder`` class responsible for encoding the actual serial signal used by sbus protocol to set an RC control value.

        By creating such instance, the gremsy gimbal thread is created and started.
        """

        self.sbus = SBUSEncoder()
        self.sbus.start_sbus(serial_interface='COM20', period_packet=0.015)

        self.define_home_position()

    def setPosControl(self, yaw, pitch, roll=0, mode=0x00, model='linear'):
        """
        Moves Gremsy H16 gimbal by the input angle. This function works as the equivalent to the ``setPosControl`` function of the class ``GimbalRS2``. If movement is desired in both yaw and pitch, the gimbal will move first in either axis (i.e. yaw) and after finishing the that movement, it will move in the other axis (i.e. pitch).

        If the linear model is used: this function sets an RC control value ("speed") and finds the corresponding time, given the angle desired to be set.

        If the gp model is used: this function calls ``setPosControlGPModel`` to get the RC control value and time, corresponding to the input angle.

        For a more accurate gimbal angle movement: 

        1. Provide a finer and equally sampled grid of RC control value, time with its corresponding measured angle.
        2. Modify ``fit_model_to_gimbal_angular_data`` to tune parameters of the gp model (if gp model is chosen).
        3. Modify  ``setPosControlGPModel`` (if gp model is chosen) if a better logic for getting RC control values and times from a given angle is available.

        Args:
            yaw (float): yaw angle (in degrees) to be set. Valid range between [-180, 180].
            pitch (float): pitch angle (in degrees) to be set. Valid range between [-90, 90].
            roll (int, optional): roll angle (in degrees) to be set. Roll angle is not used because desired movement of the gimbal only requires yaw and pitch angles. Defaults to 0.
            mode (hexadecimal, optional): to be implemented. The idea is to have two modes as in the GimbalRS2 class: ``relative`` and ``absolute``. Defaults to 0x00.
            model (str, optional): ``linear`` or ``gp`` model for the angle dependence on the RC control value and the time to hold it. For the ``gp`` model, the functionality must be implemented. Defaults to 'linear'.
        """

        if model == 'linear':
            # Choose speed for yaw movement
            if yaw > 0:
                speed_yaw = -11

                # Linear regresion model for angle dependence. If different, change this line
                time_yaw_2_move = (yaw - self.az_speed_neg_regresor.coef_[0]*speed_yaw - self.az_speed_neg_regresor.intercept_)/self.az_speed_neg_regresor.coef_[1]
            elif yaw < 0:
                speed_yaw = 15

                # Linear regresion model for angle dependence. If different, change this line
                time_yaw_2_move = (np.abs(yaw) - self.az_speed_pos_regresor.coef_[0]*speed_yaw - self.az_speed_pos_regresor.intercept_)/self.az_speed_pos_regresor.coef_[1]
            elif yaw == 0:
                time_yaw_2_move = 0

            # Choose speed for pitch movement
            if pitch > 0:
                #print("[DEBUG]: Only negative pitch values are allowed")
                speed_pitch = 10
                time_pitch_2_move = (pitch - self.el_speed_pos_regresor.coef_[0]*speed_pitch - self.el_speed_pos_regresor.intercept_)/self.el_speed_pos_regresor.coef_[1]
                #return
            elif pitch < 0:
                speed_pitch = -5
                time_pitch_2_move = (np.abs(pitch) - self.el_speed_neg_regresor.coef_[0]*speed_pitch - self.el_speed_neg_regresor.intercept_)/self.el_speed_neg_regresor.coef_[1]
            elif pitch == 0:
                time_pitch_2_move = 0

        elif model == 'gp':
            if yaw == 0:
                yaw = None
            else:
                yaw = float(yaw)
            if pitch == 0:
                pitch = None
            else:
                pitch = float(pitch)

            speed_yaw, time_yaw_2_move, speed_pitch, time_pitch_2_move = self.setPosControlGPModel(yaw=yaw, pitch=yaw)

        if (time_yaw_2_move > 0) and (time_pitch_2_move > 0):
            print("[DEBUG]: Gremsy H16 moves in yaw first: TIME to complete movement: ", time_yaw_2_move)
            self.sbus.move_gimbal(0, speed_yaw, time_yaw_2_move)
            print("[DEBUG]: Gremsy H16 moves in elevation second: TIME to complete movement: ", time_pitch_2_move)
            self.sbus.move_gimbal(speed_pitch, 0, time_pitch_2_move)
        elif (time_yaw_2_move > 0) and (time_pitch_2_move <= 0):
            print("[DEBUG]: Gremsy H16 moves in yaw: TIME to complete movement: ", time_yaw_2_move)
            self.sbus.move_gimbal(0, speed_yaw, time_yaw_2_move)
        elif (time_yaw_2_move <= 0) and (time_pitch_2_move > 0):
            print("[DEBUG]: Gremsy H16 moves in elevation: TIME to complete movement: ", time_pitch_2_move)
            self.sbus.move_gimbal(speed_pitch, 0, time_pitch_2_move)
        elif (time_yaw_2_move <= 0) and (time_pitch_2_move <= 0):
            print("[DEBUG]: Gremsy H16 will not move")
            return

    def stop_thread_gimbal(self):
        """
        Stops the imu thread and the ``SBUSEncoder`` gremsy gimbal thread.
        """
        self.stop_thread_imu()
        self.sbus.stop_updating()

    def control_power_motors(self, power='on'):
        """
        Turns gremsy gimbal motors on or off. This function is a wrapper of ``turn_on_motors`` an ``turn_off_motors`` of the ``SBUSEncoder`` class.

        Args:
            power (str, optional): ``on`` to power the motors, or ``off`` to shut them down. Defaults to 'on'.
        """

        if power == 'on':
            self.sbus.turn_on_motors()
        elif power == 'off':
            self.sbus.turn_off_motors()

    def change_gimbal_mode(self, mode='LOCK'):
        """
        Changes Gremsy gimbal mode. Available modes are: 'Lock' and 'Follow' (and motors off). Brief description of the modes is on the manual of the gimbal provided by the manufacturer.

        Args:
            mode (str, optional): either ``LOCK`` or ``FOLLOW``. Check the manual of the Gremsy H16 gimbal to understand its modes. Defaults to ``LOCK``.
        """

        self.sbus.change_mode(mode=mode)
        self.sbus.MODE = mode

__init__(speed_time_azimuth_table=None, speed_time_elevation_table=None)

Constructor for the class.

Loads the measured angle (yaw and pitch, each one separately) dependence on RC control value and time. Replace load_measured_data_august_2023 or reimplement it if newer/better measurements of this dependence are available.

Parameters:

Name	Type	Description	Default
speed_time_azimuth_table	ndarray	array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured azimuth (yaw). Defaults to None.	None
speed_time_elevation_table	ndarray	array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured elevation (pitch). Defaults to None.	None

Source code in a2gmeasurements.py

def __init__(self, speed_time_azimuth_table=None, speed_time_elevation_table=None):
    """
    Constructor for the class. 

    Loads the measured angle (yaw and pitch, each one separately) dependence on RC control value and time. Replace ``load_measured_data_august_2023`` or reimplement it if newer/better measurements of this dependence are available. 

    Args:
        speed_time_azimuth_table (numpy.ndarray, optional): array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured azimuth (yaw). Defaults to None.
        speed_time_elevation_table (numpy.ndarray, optional): array with whose 3 columns are: 1. RC control value (equivalent to speed). 2. Time holding the RC value. 3. Measured elevation (pitch). Defaults to None.
    """

    self.cnt_imu_readings = 0

    if speed_time_azimuth_table is not None:
        self.speed_time_azimuth_table = speed_time_azimuth_table
        self.speed_time_elevation_table = speed_time_elevation_table
    else:
        #self.load_measured_data_july_2023()
        self.load_measured_data_august_2023()

    # Assume linear fit:
    # For a longer range of speeds than the used in the default data, it is very likely that the fit won't be linear
    self.fit_model_to_gimbal_angular_data(model='gp')    
    self.start_imu_thread()

change_gimbal_mode(mode='LOCK')

Changes Gremsy gimbal mode. Available modes are: 'Lock' and 'Follow' (and motors off). Brief description of the modes is on the manual of the gimbal provided by the manufacturer.

Parameters:

Name	Type	Description	Default
mode	str	either LOCK or FOLLOW. Check the manual of the Gremsy H16 gimbal to understand its modes. Defaults to LOCK.	'LOCK'

Source code in a2gmeasurements.py

def change_gimbal_mode(self, mode='LOCK'):
    """
    Changes Gremsy gimbal mode. Available modes are: 'Lock' and 'Follow' (and motors off). Brief description of the modes is on the manual of the gimbal provided by the manufacturer.

    Args:
        mode (str, optional): either ``LOCK`` or ``FOLLOW``. Check the manual of the Gremsy H16 gimbal to understand its modes. Defaults to ``LOCK``.
    """

    self.sbus.change_mode(mode=mode)
    self.sbus.MODE = mode

control_power_motors(power='on')

Turns gremsy gimbal motors on or off. This function is a wrapper of turn_on_motors an turn_off_motors of the SBUSEncoder class.

Parameters:

Name	Type	Description	Default
power	str	on to power the motors, or off to shut them down. Defaults to 'on'.	'on'

Source code in a2gmeasurements.py

def control_power_motors(self, power='on'):
    """
    Turns gremsy gimbal motors on or off. This function is a wrapper of ``turn_on_motors`` an ``turn_off_motors`` of the ``SBUSEncoder`` class.

    Args:
        power (str, optional): ``on`` to power the motors, or ``off`` to shut them down. Defaults to 'on'.
    """

    if power == 'on':
        self.sbus.turn_on_motors()
    elif power == 'off':
        self.sbus.turn_off_motors()

define_home_position()

Defines the center of the coordinated system (yaw, pitch). To do so, it checks if the gimbal is moving (in both azimuth and elevation) and if not, read the angles provided by the IMU and set them as the home position. Due to this operation, this function must be called in __init__ or before any setPosControl call.

As the gimbal controller from the manufacturer can't be accesed, imu readings are not available. An external IMU is required and any cheap raspberry pi pico is capable of providing decent IMU support. Potential magnetic interferences between the IMU readings of the raspberry pi pico and the motors of the gimbal have not been researched.

NOTE: this function requires to be further tested, if the Gremsy gimbal is to be used again as part of the channel sounder system. The reason is that when checking if the gimbal is moving, a good tolerated error (tol_err = abs(angle_now - angle_before)) must be set.

Source code in a2gmeasurements.py

def define_home_position(self):
    """
    Defines the center of the coordinated system (yaw, pitch). To do so, it checks if the gimbal is moving (in both azimuth and elevation) and if not, read the angles provided by the IMU and set them as the home position. Due to this operation, this function **must** be called in ``__init__`` or before any ``setPosControl`` call.

    As the gimbal controller from the manufacturer can't be accesed, imu readings are not available. An external IMU is required and any cheap raspberry pi pico is capable of providing decent IMU support. Potential magnetic interferences between the IMU readings of the raspberry pi pico and the motors of the gimbal have not been researched.

    **NOTE**: this function *requires to be further tested*, if the Gremsy gimbal is to be used again as part of the channel sounder system. The reason is that when checking if the gimbal is moving, a good tolerated error (tol_err = abs(angle_now - angle_before)) must be set.
    """
    print("[DEBUG]: Defining HOME for Gremsy... This might take a second")

    start_time = time.time()
    yaw_before = 1000 # Set it high for first iteration
    pitch_before = 1000 # Set it high for first iteration
    cnt = self.cnt_imu_readings
    DONT_STOP = True
    tol_err = 2
    while(DONT_STOP):
        if cnt != self.cnt_imu_readings:
            yaw = self.last_imu_reading['YAW']
            pitch = self.last_imu_reading['PITCH']

            if np.abs(yaw - yaw_before) <= tol_err:
                CONDITION_YAW_SATISFIED = True
            else:
                CONDITION_YAW_SATISFIED = False

            if np.abs(pitch - pitch_before) <= tol_err:
                CONDITION_PITCH_SATISFIED = True
            else:
                CONDITION_PITCH_SATISFIED = False

            if CONDITION_YAW_SATISFIED and CONDITION_PITCH_SATISFIED:
                break        

            yaw_before = yaw
            pitch_before = pitch
            cnt = self.cnt_imu_readings

    self.home_position = {'YAW': yaw, 'PITCH': pitch}
    print(f"TIME SPENT DEFINE HOME POSITION: {time.time() - start_time}")

fit_model_to_gimbal_angular_data(model='linear')

Fits a model to the measured angle (yaw, pitch) dependence on time and speed (RC control value). There are two models: a linear model and a gaussian regressor.

The linear model is suitable specific range of "speeds" and time, since the non-linear dependence can be linearized. However, this range must be defined.

For the gaussian regressor, more training samples (RC control value, time, angle) over the full range are required to avoid overfitting and bad predicting behaviour.

There is either a linear or gp model for the RC control positive values and another one for the RC control negative values.

Parameters:

Name	Type	Description	Default
model	str	either linear or gp. Defaults to linear.	'linear'

Source code in a2gmeasurements.py

def fit_model_to_gimbal_angular_data(self, model='linear'):
    """
    Fits a model to the measured angle (yaw, pitch) dependence on time and speed (RC control value). There are two models: a linear model and a gaussian regressor.

    The linear model is suitable specific range of "speeds" and time, since the non-linear dependence can be linearized. However, this *range must be defined*.

    For the gaussian regressor, *more training samples (RC control value, time, angle) over the full range are required* to avoid overfitting and bad predicting behaviour.

    There is either a linear or gp model for the RC control positive values and another one for the RC control negative values.

    Args:
        model (str, optional): either ``linear`` or ``gp``. Defaults to ``linear``.
    """

    # Define the kernel for Gaussian Process Regressor
    kernel = 1.0 * RBF(length_scale=1.0, length_scale_bounds=(1e-2, 1e2)) + WhiteKernel(noise_level=1e-5, noise_level_bounds=(1e-10, 1e-3))

    is_positive_azimuth = self.speed_time_azimuth_table > 0
    is_positive_elevation = self.speed_time_elevation_table > 0

    X = self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 0:2]
    y = np.rad2deg(self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 2])
    if model =='linear':
        self.az_speed_pos_regresor = LinearRegression().fit(X, y)
    elif model == 'gp':
        self.az_speed_pos_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y)
    self.score_az_speed_pos_regresor = self.az_speed_pos_regresor.score(X, y)
    print("[DEBUG]: POSITIVE SPEEDS (LEFT), AZIMUTH, R^2 Score Linear Reg: ", self.score_az_speed_pos_regresor)

    X = self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 0:2]
    y = np.rad2deg(self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 2])
    if model == 'linear':
        self.az_speed_neg_regresor = LinearRegression().fit(X, y)
    elif model == 'gp':
        self.az_speed_neg_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y)
    self.score_az_speed_neg_regresor = self.az_speed_neg_regresor.score(X, y)
    print("[DEBUG]: NEGATIVE SPEEDS (RIGHT), AZIMUTH, R^2 Score Linear Reg: ", self.score_az_speed_neg_regresor)

    X = self.speed_time_elevation_table[~is_positive_elevation[:, 0], 0:2]
    y = np.rad2deg(self.speed_time_elevation_table[~is_positive_elevation[:, 0], 2])
    if model == 'linear':
        self.el_speed_neg_regresor = LinearRegression().fit(X, y)
    elif model == 'gp':
        self.el_speed_neg_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y).fit(X, y)
    self.score_el_speed_neg_regresor = self.el_speed_neg_regresor.score(X, y)
    print("[DEBUG]: NEGATIVE SPEEDS (DOWN), ELEVATION, R^2 Score Linear Reg: ", self.score_el_speed_neg_regresor)

    X = self.speed_time_elevation_table[is_positive_elevation[:, 0], 0:2]
    y = np.rad2deg(self.speed_time_elevation_table[is_positive_elevation[:, 0], 2])
    if model == 'linear':
        self.el_speed_pos_regresor = LinearRegression().fit(X, y)
    elif model == 'gp':
        self.el_speed_pos_regresor = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=10, random_state=42).fit(X, y).fit(X, y)
    self.score_el_speed_pos_regresor = self.el_speed_pos_regresor.score(X, y)
    print("[DEBUG]: POSITIVE SPEEDS (UP), ELEVATION, R^2 Score Linear Reg: ", self.score_el_speed_neg_regresor)

gremsy_angle(a_i, a_ip, b_i)

Computes the angle between the sides of the triangle given by a_i and a_i+1. The opposite side to the angle computed is the one defined by b_i.

A definition of this distances can be found in "Manual A2GMeasurements".

NOTE FOR DEVELOPERS: the experiment described in * "Manual A2GMeasurements" was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the receive_imu_data function of this class*.

Parameters:

Name	Type	Description	Default
a_i	float	defined in "Manual A2GMeasurements".	required
a_ip	float	defined in "Manual A2GMeasurements".	required
b_i	float	defined in "Manual A2GMeasurements".	required

Returns:

Name	Type	Description
Radians	float	mentioned angle in radians.

Source code in a2gmeasurements.py

def gremsy_angle(self, a_i, a_ip, b_i):
    """
    Computes the angle between the sides of the triangle given by a_i and  a_i+1. The opposite side to the angle computed is the one defined by b_i. 

    A definition of this distances can be found in "Manual A2GMeasurements".

    NOTE FOR DEVELOPERS: *the experiment described in * "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.

    Args:
        a_i (float): defined in "Manual A2GMeasurements".
        a_ip (float): defined in "Manual A2GMeasurements".
        b_i (float): defined in "Manual A2GMeasurements".

    Returns:
        Radians (float): mentioned angle in radians.
    """

    tmp = (a_i**2 + a_ip**2 - b_i**2)/(2*a_i*a_ip)
    return np.arccos(tmp)

load_measured_data_august_2023()

Loads a second set of measured data extracted from the experiment described on "Manual A2GMeasurements".

This table contains as columns the speed [-100, 100], time [s], and the angle (azimuth, elevation) computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

NOTE FOR DEVELOPERS: the experiment described in "Manual A2GMeasurements" was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the receive_imu_data function of this class.

Source code in a2gmeasurements.py

def load_measured_data_august_2023(self):
    """
    Loads a second set of measured data extracted from the experiment described on "Manual A2GMeasurements".

    This table contains as columns the speed [-100, 100], time [s], and the angle (azimuth, elevation) computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

    NOTE FOR DEVELOPERS: *the experiment described in* "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.
    """        
    speed_time_azimuth_table = [[15, 6, self.gremsy_angle(1.903, 1.949, 0.87)], 
                    [15, 7, self.gremsy_angle(1.955, 1.926, 1)],
                    [15, 8, self.gremsy_angle(2.071, 1.897, 1.19)],
                    [15, 9, self.gremsy_angle(2.023, 1.949, 1.315)],
                    [16, 5, self.gremsy_angle(1.879, 2.078, 0.875)],
                    [16, 6, self.gremsy_angle(1.883, 2.069, 1.025)],
                    [16, 7, self.gremsy_angle(1.897, 2.219, 1.26)],
                    [14, 7, self.gremsy_angle(1.886, 1.994, 0.86)],
                    [14, 8, self.gremsy_angle(1.881, 2.069, 1)],
                    [14, 9, self.gremsy_angle(1.888, 2.086, 1.134)],
                    [-14, 4, self.gremsy_angle(1.922, 2.047, 1.255)],
                    [-14, 5, self.gremsy_angle(1.961, 2.117, 1.59)],
                    [-14, 6, self.gremsy_angle(2.106, 2.089, 1.93)],
                    [-13, 4, self.gremsy_angle(2.034, 1.909, 1.165)],
                    [-13, 5, self.gremsy_angle(2.025, 1.985, 1.44)],
                    [-13, 6, self.gremsy_angle(2.183, 1.98, 1.79)],
                    [-15, 4, self.gremsy_angle(2.38, 1.574, 1.666)],
                    [-15, 5, self.gremsy_angle(2.183, 1.538, 1.836)],
                    [-15, 6, self.gremsy_angle(2.183, 1.546, 2.111)],
                    [-16, 4, self.gremsy_angle(2.183, 1.558, 1.648)],
                    [-16, 5, self.gremsy_angle(2.183, 1.537, 1.934)],
                    [-16, 6, self.gremsy_angle(2.183, 1.562, 2.215)],
                    [-17, 4, self.gremsy_angle(2.183, 1.55, 1.71)],
                    [-17, 5, self.gremsy_angle(2.183, 1.54, 2.103)],
                    [-17, 6, self.gremsy_angle(2.183, 1.586, 2.327)],
                    [-18, 4, self.gremsy_angle(2.183, 1.541, 1.799)],
                    [-18, 5, self.gremsy_angle(2.183, 1.623, 2.109)],
                    [-18, 6, self.gremsy_angle(2.183, 1.623, 2.463)],
                    [-19, 4, self.gremsy_angle(2.183, 1.537, 1.885)],
                    [-19, 5, self.gremsy_angle(2.183, 1.563, 2.215)],
                    [-19, 6, self.gremsy_angle(2.183, 1.663, 2.582)],
                    [-20, 4, self.gremsy_angle(2.183, 1.537, 1.952)],
                    [-20, 5, self.gremsy_angle(2.183, 1.585, 2.321)],
                    [-20, 6, self.gremsy_angle(2.183, 1.718, 2.72)]]
    self.speed_time_azimuth_table = np.array(speed_time_azimuth_table)

    speed_time_elevation_table = [[10, 3, self.gremsy_angle(1.526, 1.529, 0.07)],
                                    [10, 4, self.gremsy_angle(1.526, 1.531, 0.096)],
                                    [10, 5, self.gremsy_angle(1.526, 1.532, 0.11)],
                                    [10, 6, self.gremsy_angle(1.526, 1.533, 0.122)],
                                    [10, 7, self.gremsy_angle(1.526, 1.537, 0.16)],
                                    [10, 8, self.gremsy_angle(1.526, 1.540, 0.183)],
                                    [11, 3, self.gremsy_angle(1.526, 1.543, 0.124)],
                                    [11, 4, self.gremsy_angle(1.526, 1.548, 0.163)],
                                    [11, 5, self.gremsy_angle(1.526, 1.542, 0.199)],
                                    [11, 6, self.gremsy_angle(1.526, 1.554, 0.239)],
                                    [11, 7, self.gremsy_angle(1.526, 1.548, 0.279)],
                                    [12, 3, self.gremsy_angle(1.526, 1.538, 0.168)],
                                    [12, 4, self.gremsy_angle(1.526, 1.545, 0.227)],
                                    [12, 5, self.gremsy_angle(1.526, 1.555, 0.283)],
                                    [12, 6, self.gremsy_angle(1.526, 1.568, 0.343)],
                                    [12, 7, self.gremsy_angle(1.526, 1.580, 0.4)],
                                    [13, 3, self.gremsy_angle(1.526, 1.548, 0.244)],
                                    [13, 4, self.gremsy_angle(1.526, 1.56, 0.311)],
                                    [13, 5, self.gremsy_angle(1.526, 1.578, 0.395)],
                                    [13, 6, self.gremsy_angle(1.526, 1.598, 0.448)],
                                    [13, 7, self.gremsy_angle(1.526, 1.656, 0.64)],
                                    [14, 2, self.gremsy_angle(1.526, 1.542, 0.184)],
                                    [14, 3, self.gremsy_angle(1.526, 1.558, 0.286)],
                                    [14, 4, self.gremsy_angle(1.526, 1.578, 0.378)],
                                    [14, 5, self.gremsy_angle(1.526, 1.603, 0.474)],
                                    [14, 6, self.gremsy_angle(1.526, 1.635, 0.574)],
                                    [15, 1, self.gremsy_angle(1.526, 1.531, 0.11)],
                                    [15, 2, self.gremsy_angle(1.526, 1.542, 0.209)],
                                    [15, 3, self.gremsy_angle(1.526, 1.565, 0.334)],
                                    [15, 4, self.gremsy_angle(1.526, 1.594, 0.448)],
                                    [15, 5, self.gremsy_angle(1.526, 1.63, 0.567)],
                                    [16, 1, self.gremsy_angle(1.526, 1.533, 0.125)],
                                    [16, 2, self.gremsy_angle(1.526, 1.549, 0.253)],
                                    [16, 3, self.gremsy_angle(1.526, 1.575, 0.378)],
                                    [16, 4, self.gremsy_angle(1.526, 1.614, 0.519)],
                                    [16, 5, self.gremsy_angle(1.526, 1.666, 0.665)],
                                    [17, 1, self.gremsy_angle(1.526, 1.535, 0.147)],
                                    [17, 2, self.gremsy_angle(1.526, 1.557, 0.291)],
                                    [17, 3, self.gremsy_angle(1.526, 1.59, 0.435)],
                                    [17, 4, self.gremsy_angle(1.526, 1.642, 0.6)],
                                    [17, 5, self.gremsy_angle(1.526, 1.708, 0.766)],
                                    [18, 1, self.gremsy_angle(1.526, 1.537, 0.164)],
                                    [18, 2, self.gremsy_angle(1.526, 1.563, 0.328)],
                                    [18, 3, self.gremsy_angle(1.526, 1.608, 0.502)],
                                    [18, 4, self.gremsy_angle(1.526, 1.676, 0.693)],
                                    [18, 5, self.gremsy_angle(1.526, 1.761, 0.884)],
                                    [19, 0.5, self.gremsy_angle(1.526, 1.529, 0.088)],
                                    [19, 1, self.gremsy_angle(1.526, 1.538, 0.176)],
                                    [19, 2, self.gremsy_angle(1.526, 1.571, 0.363)],
                                    [19, 3, self.gremsy_angle(1.526, 1.626, 0.558)],
                                    [19, 4, self.gremsy_angle(1.526, 1.707, 0.766)],
                                    [20, 0.5, self.gremsy_angle(1.526, 1.53, 0.1)],
                                    [20, 1, self.gremsy_angle(1.526, 1.541, 0.2)],
                                    [20, 2, self.gremsy_angle(1.526, 1.579, 0.395)],
                                    [20, 3, self.gremsy_angle(1.526, 1.648, 0.616)],
                                    [20, 4, self.gremsy_angle(1.526, 1.742, 0.845)],
                                    [-5, 3, self.gremsy_angle(1.769, 1.652, 0.268)],
                                    [-5, 6, self.gremsy_angle(1.769, 1.58, 0.5)],
                                    [-5, 7, self.gremsy_angle(1.769, 1.562, 0.575)],
                                    [-5, 8, self.gremsy_angle(1.769, 1.548, 0.648)],
                                    [-5, 10, self.gremsy_angle(1.769, 1.534, 0.758)],
                                    [-6, 3, self.gremsy_angle(1.769, 1.632, 0.324)],
                                    [-6, 6, self.gremsy_angle(1.769, 1.556, 0.608)],
                                    [-6, 8, self.gremsy_angle(1.769, 1.531, 0.785)],
                                    [-6, 10, self.gremsy_angle(1.769, 1.527, 0.958)],
                                    [-7, 3, self.gremsy_angle(1.769, 1.713, 0.385)],
                                    [-7, 6, self.gremsy_angle(1.769, 1.534, 0.717)],
                                    [-7, 8, self.gremsy_angle(1.769, 1.526, 0.925)],
                                    [-7, 10, self.gremsy_angle(1.769, 1.541, 1.135)],
                                    [-8, 3, self.gremsy_angle(1.769, 1.599, 0.433)],
                                    [-8, 6, self.gremsy_angle(1.769, 1.53, 0.983)],
                                    [-8, 8, self.gremsy_angle(1.769, 1.533, 1.058)],
                                    [-8, 10, self.gremsy_angle(1.769, 1.574, 1.309)],
                                    [-9, 3, self.gremsy_angle(1.769, 1.581, 0.496)],
                                    [-9, 6, self.gremsy_angle(1.769, 1.527, 0.934)],
                                    [-9, 8, self.gremsy_angle(1.769, 1.554, 1.214)],
                                    [-9, 9, self.gremsy_angle(1.769, 1.588, 1.364)],
                                    [-10, 2, self.gremsy_angle(1.769, 1.612, 0.387)],
                                    [-10, 5, self.gremsy_angle(1.769, 1.527, 0.883)],
                                    [-10, 7, self.gremsy_angle(1.769, 1.551, 1.197)],
                                    [-10, 8, self.gremsy_angle(1.769, 1.588, 1.364)],
                                    [-11, 2, self.gremsy_angle(1.769, 1.601, 0.42)],
                                    [-11, 5, self.gremsy_angle(1.769, 1.527, 0.96)],
                                    [-11, 6, self.gremsy_angle(1.769, 1.541, 1.136)],
                                    [-11, 7, self.gremsy_angle(1.769, 1.577, 1.322)],
                                    [-12, 2, self.gremsy_angle(1.769, 1.588, 0.467)],
                                    [-12, 5, self.gremsy_angle(1.769, 1.532, 1.048)],
                                    [-12, 6, self.gremsy_angle(1.769, 1.559, 1.247)],
                                    [-12, 7, self.gremsy_angle(1.769, 1.611, 1.446)],
                                    [-13, 2, self.gremsy_angle(1.769, 1.579, 0.502)],
                                    [-13, 5, self.gremsy_angle(1.769, 1.542, 1.144)],
                                    [-13, 6, self.gremsy_angle(1.769, 1.588, 1.363)],
                                    [-13, 7, self.gremsy_angle(1.769, 1.672, 1.611)],
                                    [-14, 2, self.gremsy_angle(1.769, 1.572, 0.534)],
                                    [-14, 5, self.gremsy_angle(1.769, 1.558, 1.231)],
                                    [-14, 6, self.gremsy_angle(1.769, 1.624, 1.483)],
                                    [-14, 7, self.gremsy_angle(1.769, 1.672, 1.752)],
                                    [-15, 2, self.gremsy_angle(1.769, 1.565, 0.559)],
                                    [-15, 4, self.gremsy_angle(1.769, 1.534, 1.069)],
                                    [-15, 5, self.gremsy_angle(1.769, 1.58, 1.328)],
                                    [-15, 6, self.gremsy_angle(1.769, 1.666, 1.6)],
                                    [-16, 2, self.gremsy_angle(1.769, 1.558, 0.601)],
                                    [-16, 4, self.gremsy_angle(1.769, 1.541, 1.136)],
                                    [-16, 5, self.gremsy_angle(1.769, 1.604, 1.42)],
                                    [-16, 6, self.gremsy_angle(1.769, 1.722, 1.73)],
                                    [-17, 2, self.gremsy_angle(1.769, 1.55, 0.641)],
                                    [-17, 4, self.gremsy_angle(1.769, 1.554, 1.212)],
                                    [-17, 5, self.gremsy_angle(1.769, 1.634, 1.512)],
                                    [-17, 6, self.gremsy_angle(1.769, 1.74, 1.771)],
                                    [-18, 1, self.gremsy_angle(1.769, 1.623, 0.355)],
                                    [-18, 3, self.gremsy_angle(1.769, 1.528, 0.98)],
                                    [-18, 4, self.gremsy_angle(1.769, 1.568, 1.284)],
                                    [-18, 5, self.gremsy_angle(1.769, 1.677, 1.625)],
                                    [-19, 1, self.gremsy_angle(1.769, 1.615, 0.38)],
                                    [-19, 3, self.gremsy_angle(1.769, 1.531, 1.031)],
                                    [-19, 4, self.gremsy_angle(1.769, 1.591, 1.374)],
                                    [-19, 5, self.gremsy_angle(1.769, 1.726, 1.774)],
                                    [-20, 1, self.gremsy_angle(1.769, 1.609, 0.401)],
                                    [-20, 3, self.gremsy_angle(1.769, 1.536, 1.086)],
                                    [-20, 4, self.gremsy_angle(1.769, 1.613, 1.451)],
                                    [-20, 5, self.gremsy_angle(1.769, 1.737, 1.764)]]
    self.speed_time_elevation_table = np.array(speed_time_elevation_table)

load_measured_data_july_2023()

Loads a set of measured data extracted from the experiment described on "Manual A2GMeasurements".

This table contains as columns the speed [-100, 100], time [s], and the azimuth angle computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

NOTE FOR DEVELOPERS: the experiment described in "Manual A2GMeasurements" was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the receive_imu_data function of this class.

Source code in a2gmeasurements.py

def load_measured_data_july_2023(self):
    """
    Loads a set of measured data extracted from the experiment described on "Manual A2GMeasurements". 

    This table contains as columns the speed [-100, 100], time [s], and the azimuth angle computed from the 3 distances (a_{i}, a_{i+1}, b_{i}) described in "Manual A2GMeasurements".

    NOTE FOR DEVELOPERS: *the experiment described in* "Manual A2GMeasurements" *was done before acquiring the external IMU. With the use of the external IMU a much easier measurement of the yaw and pitch can be done using (and extending) the ``receive_imu_data`` function of this class*.
    """        
    speed_time_azimuth_table = [[15, 6, self.gremsy_angle(1.903, 1.949, 0.87)], 
                    [15, 7, self.gremsy_angle(1.955, 1.926, 1)],
                    [15, 8, self.gremsy_angle(2.071, 1.897, 1.19)],
                    [15, 9, self.gremsy_angle(2.023, 1.949, 1.315)],
                    [16, 5, self.gremsy_angle(1.879, 2.078, 0.875)],
                    [16, 6, self.gremsy_angle(1.883, 2.069, 1.025)],
                    [16, 7, self.gremsy_angle(1.897, 2.219, 1.26)],
                    [14, 7, self.gremsy_angle(1.886, 1.994, 0.86)],
                    [14, 8, self.gremsy_angle(1.881, 2.069, 1)],
                    [14, 9, self.gremsy_angle(1.888, 2.086, 1.134)],
                    [-14, 4, self.gremsy_angle(1.922, 2.047, 1.255)],
                    [-14, 5, self.gremsy_angle(1.961, 2.117, 1.59)],
                    [-14, 6, self.gremsy_angle(2.106, 2.089, 1.93)],
                    [-13, 4, self.gremsy_angle(2.034, 1.909, 1.165)],
                    [-13, 5, self.gremsy_angle(2.025, 1.985, 1.44)],
                    [-13, 6, self.gremsy_angle(2.183, 1.98, 1.79)]]
    self.speed_time_azimuth_table = np.array(speed_time_azimuth_table)

    speed_time_elevation_table = [[-5, 1, self.gremsy_angle(1.884, 1.882, 0.1)],
                                        [-5, 2, self.gremsy_angle(1.881, 1.889, 0.175)],
                                        [-5, 3, self.gremsy_angle(1.89, 1.934, 0.272)],
                                        [-5, 4, self.gremsy_angle(1.889, 1.891, 0.345)]]
    self.speed_time_elevation_table = np.array(speed_time_elevation_table)        

load_measured_drifts()

Loads measured drift angles from the experiment described on "Manual A2GMeasurements".

Source code in a2gmeasurements.py

def load_measured_drifts(self):
    """
    Loads measured drift angles from the experiment described on "Manual A2GMeasurements".        
    """ 
    drift_with_low_speed_counter = [[137, self.gremsy_angle(2, 1.97, 0.10)],
                                    [144, self.gremsy_angle(1.97, 1.94, 0.10)],
                                    [145, self.gremsy_angle(1.94, 1.927, 0.10)],
                                    [156, self.gremsy_angle(1.927, 1.911, 0.10)],
                                    [164, self.gremsy_angle(1.911, 1.898, 0.10)],
                                    [148, self.gremsy_angle(1.898, 1.892, 0.10)],
                                    [164, self.gremsy_angle(1.892, 1.89, 0.10)],
                                    [176, self.gremsy_angle(1.89, 1.894, 0.10)],
                                    [185, self.gremsy_angle(1.894, 1.9, 0.10)],
                                    [159, self.gremsy_angle(1.9, 1.914, 0.10)],
                                    [159, self.gremsy_angle(1.914, 1.932, 0.10)],
                                    [146, self.gremsy_angle(1.932, 1.954, 0.10)]]

    drift_without_low_speed_counter = [[28.91, self.gremsy_angle(2, 1.971, 0.10)],
                                       [28.43, self.gremsy_angle(1.971, 1.95, 0.10)],
                                       [28.46, self.gremsy_angle(1.95, 1.923, 0.10)],
                                       [30.14, self.gremsy_angle(1.923, 1.9, 0.10)],
                                       [29.76, self.gremsy_angle(1.9, 1.888, 0.10)],
                                       [29.36, self.gremsy_angle(1.888, 1.884, 0.10)],
                                       [31.41, self.gremsy_angle(1.884, 1.872, 0.10)],
                                       [31.3, self.gremsy_angle(1.872, 1.881, 0.10)],
                                       [28.77, self.gremsy_angle(1.881, 1.89, 0.10)],
                                       [31.56, self.gremsy_angle(1.89, 1.912, 0.10)],
                                       [29.15, self.gremsy_angle(1.912, 1.935, 0.10)]]

    self.drift_with_low_speed_counter = drift_with_low_speed_counter
    self.drift_without_low_speed_counter = drift_without_low_speed_counter
    self.avg_drift_with_low_speed_counter = np.array(drift_with_low_speed_counter).mean(axis=0) # 2D array 
    self.avg_drift_without_low_speed_counter = np.array(drift_without_low_speed_counter).mean(axis=0) # 2D array

plot_linear_reg_on_near_domain(loaded='august')

Generates a figure with 3 subplots with measured values (default measured values or new values measured for more (speed, time) tuples given as a parameter to the class) and with linear regression model applied to the measured values.

1. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> azimuth
2. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> azimuth
3. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> elevation

If loaded is august then the figure has 4 subplots, with the 4th being 4. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> elevation

Parameters:

Name	Type	Description	Default
loaded	str	which table loaded: the one from load_measured_data_july_2023 or the one from load_measured_data_august_2023. Defaults to august.	'august'

Source code in a2gmeasurements.py

def plot_linear_reg_on_near_domain(self, loaded='august'):
    """
    Generates a figure with 3 subplots with measured values (default measured values or new values measured for more (speed, time) tuples given as a parameter to the class) and with linear regression model applied to the measured values.

    1. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> azimuth 
    2. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> azimuth 
    3. 3D Scatter Plot of (speed, time, angle) for speed < 0 and angle -> elevation

    If ``loaded`` is august then the figure has 4 subplots, with the 4th being
    4. 3D Scatter Plot of (speed, time, angle) for speed > 0 and angle -> elevation

    Args:
        loaded (str, optional): which table loaded: the one from ``load_measured_data_july_2023`` or the one from ``load_measured_data_august_2023``. Defaults to ``august``.
    """

    is_positive_azimuth = self.speed_time_azimuth_table > 0
    is_positive_elevation = self.speed_time_elevation_table > 0       

    # We are going to use multiple if - else statements of the same kind just to maintain some readibility of the code
    if loaded == 'august':
        fig = make_subplots(rows=2, cols=2, specs=[[{"type": "scene"}, {"type": "scene"}], [{"type": "scene"}, {"type": "scene"}]],
                            subplot_titles=("Pos. Speed - Azimuth", "Neg. Speed - Azimuth", "Neg. Speed - Elevation", "Pos. Speed - Elevation"))
    else:
        fig = make_subplots(rows=1, cols=3, specs=[[{"type": "scene"}, {"type": "scene"}, {"type": "scene"}]],
                            subplot_titles=("Pos. Speed - Azimuth", "Neg. Speed - Azimuth", "Neg. Speed - Elevation"))

    X, Y = np.meshgrid(np.linspace(10, 20, num=11), np.linspace(5, 9, num=5))
    XY = np.c_[X.flatten(), Y.flatten()]
    Z = self.az_speed_pos_regresor.predict(XY)        

    fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=1, col=1)
    fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 0], y=self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 1],
            z=np.rad2deg(self.speed_time_azimuth_table[is_positive_azimuth[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=1, col=1)

    X, Y = np.meshgrid(np.linspace(-20, -10, num=11), np.linspace(4, 6, num=3))    
    XY = np.c_[X.flatten(), Y.flatten()]
    Z = self.az_speed_neg_regresor.predict(XY)

    fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=1, col=2)
    fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 0], y=self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 1],
            z=np.rad2deg(self.speed_time_azimuth_table[~is_positive_azimuth[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=1, col=2)

    if loaded == 'august':
        X, Y = np.meshgrid(np.linspace(-20, -5, num=16), np.linspace(1, 10, num=10))
        nrow = 2
        ncol = 1
    else:
        X, Y = np.meshgrid(np.linspace(-10, -5, num=6), np.linspace(1, 4, num=4))
        nrow = 1
        ncol = 3
    XY = np.c_[X.flatten(), Y.flatten()]
    Z = self.el_speed_neg_regresor.predict(XY)

    fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=nrow, col=ncol)
    fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_elevation_table[~is_positive_elevation[:, 0], 0], y=self.speed_time_elevation_table[~is_positive_elevation[:, 0], 1],
            z=np.rad2deg(self.speed_time_elevation_table[~is_positive_elevation[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=nrow, col=ncol)

    if loaded == 'august':
        X, Y = np.meshgrid(np.linspace(10, 20, num=11), np.linspace(0, 9, num=10))
        XY = np.c_[X.flatten(), Y.flatten()]
        Z = self.el_speed_pos_regresor.predict(XY)

        fig.add_trace(go.Scatter3d(mode='markers', x=XY[:, 0], y=XY[:, 1], z=Z, marker=dict(color='blue', size=5,), name='Extrapolated', showlegend=True), row=2, col=2)
        fig.add_trace(go.Scatter3d(mode='markers', x=self.speed_time_elevation_table[is_positive_elevation[:, 0], 0], y=self.speed_time_elevation_table[is_positive_elevation[:, 0], 1],
                z=np.rad2deg(self.speed_time_elevation_table[is_positive_elevation[:, 0], 2]), marker=dict(color='red', size=3,), name='Measured', showlegend=True), row=2, col=2)

    fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                    yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                    zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                    row=1, col=1,)
    fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                    yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                    zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                    row=1, col=2,)
    fig.update_scenes(xaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black",title='Speed',),
                    yaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Time [s]', ),
                    zaxis=dict(backgroundcolor="rgba(0, 0, 0,0)", gridcolor="rgba(1, 1, 1, 0.1)", showbackground=True, zerolinecolor="black", title='Angle [s]'),
                    row=1, col=3,)

    fig.update_layout(autosize=False, width=1400, height=800, margin=dict(l=50, r=50, t=50, b=50),)
    fig.show()

receive_imu_data(stop_event)

Callback function for the imu thread. Read the yaw, pitch, roll angles and stores them in the attribute last_imu_reading of this class.

Parameters:

Name	Type	Description	Default
stop_event	Event	when this is set, this function won't do anything.	required

Source code in a2gmeasurements.py

def receive_imu_data(self, stop_event):
    """
    Callback function for the imu thread. Read the yaw, pitch, roll angles and stores them in the attribute ``last_imu_reading`` of this class.

    Args:
        stop_event (threading.Event): when this is set, this function won't do anything.
    """

    while not stop_event.is_set():
        data = self.imu_serial.readline().decode('utf-8').strip()
        data = data.split(',')

        self.last_imu_reading = {'YAW': float(data[0]), 'PITCH': float(data[1]), 'ROLL': float(data[2])}
        self.cnt_imu_readings = self.cnt_imu_readings + 1

        if self.cnt_imu_readings > 1e12:
            self.cnt_imu_readings = 0

setPosControl(yaw, pitch, roll=0, mode=0, model='linear')

Moves Gremsy H16 gimbal by the input angle. This function works as the equivalent to the setPosControl function of the class GimbalRS2. If movement is desired in both yaw and pitch, the gimbal will move first in either axis (i.e. yaw) and after finishing the that movement, it will move in the other axis (i.e. pitch).

If the linear model is used: this function sets an RC control value ("speed") and finds the corresponding time, given the angle desired to be set.

If the gp model is used: this function calls setPosControlGPModel to get the RC control value and time, corresponding to the input angle.

For a more accurate gimbal angle movement:

1. Provide a finer and equally sampled grid of RC control value, time with its corresponding measured angle.
2. Modify fit_model_to_gimbal_angular_data to tune parameters of the gp model (if gp model is chosen).
3. Modify setPosControlGPModel (if gp model is chosen) if a better logic for getting RC control values and times from a given angle is available.

Parameters:

Name	Type	Description	Default
yaw	float	yaw angle (in degrees) to be set. Valid range between [-180, 180].	required
pitch	float	pitch angle (in degrees) to be set. Valid range between [-90, 90].	required
roll	int	roll angle (in degrees) to be set. Roll angle is not used because desired movement of the gimbal only requires yaw and pitch angles. Defaults to 0.	0
mode	hexadecimal	to be implemented. The idea is to have two modes as in the GimbalRS2 class: relative and absolute. Defaults to 0x00.	0
model	str	linear or gp model for the angle dependence on the RC control value and the time to hold it. For the gp model, the functionality must be implemented. Defaults to 'linear'.	'linear'

Source code in a2gmeasurements.py

def setPosControl(self, yaw, pitch, roll=0, mode=0x00, model='linear'):
    """
    Moves Gremsy H16 gimbal by the input angle. This function works as the equivalent to the ``setPosControl`` function of the class ``GimbalRS2``. If movement is desired in both yaw and pitch, the gimbal will move first in either axis (i.e. yaw) and after finishing the that movement, it will move in the other axis (i.e. pitch).

    If the linear model is used: this function sets an RC control value ("speed") and finds the corresponding time, given the angle desired to be set.

    If the gp model is used: this function calls ``setPosControlGPModel`` to get the RC control value and time, corresponding to the input angle.

    For a more accurate gimbal angle movement: 

    1. Provide a finer and equally sampled grid of RC control value, time with its corresponding measured angle.
    2. Modify ``fit_model_to_gimbal_angular_data`` to tune parameters of the gp model (if gp model is chosen).
    3. Modify  ``setPosControlGPModel`` (if gp model is chosen) if a better logic for getting RC control values and times from a given angle is available.

    Args:
        yaw (float): yaw angle (in degrees) to be set. Valid range between [-180, 180].
        pitch (float): pitch angle (in degrees) to be set. Valid range between [-90, 90].
        roll (int, optional): roll angle (in degrees) to be set. Roll angle is not used because desired movement of the gimbal only requires yaw and pitch angles. Defaults to 0.
        mode (hexadecimal, optional): to be implemented. The idea is to have two modes as in the GimbalRS2 class: ``relative`` and ``absolute``. Defaults to 0x00.
        model (str, optional): ``linear`` or ``gp`` model for the angle dependence on the RC control value and the time to hold it. For the ``gp`` model, the functionality must be implemented. Defaults to 'linear'.
    """

    if model == 'linear':
        # Choose speed for yaw movement
        if yaw > 0:
            speed_yaw = -11

            # Linear regresion model for angle dependence. If different, change this line
            time_yaw_2_move = (yaw - self.az_speed_neg_regresor.coef_[0]*speed_yaw - self.az_speed_neg_regresor.intercept_)/self.az_speed_neg_regresor.coef_[1]
        elif yaw < 0:
            speed_yaw = 15

            # Linear regresion model for angle dependence. If different, change this line
            time_yaw_2_move = (np.abs(yaw) - self.az_speed_pos_regresor.coef_[0]*speed_yaw - self.az_speed_pos_regresor.intercept_)/self.az_speed_pos_regresor.coef_[1]
        elif yaw == 0:
            time_yaw_2_move = 0

        # Choose speed for pitch movement
        if pitch > 0:
            #print("[DEBUG]: Only negative pitch values are allowed")
            speed_pitch = 10
            time_pitch_2_move = (pitch - self.el_speed_pos_regresor.coef_[0]*speed_pitch - self.el_speed_pos_regresor.intercept_)/self.el_speed_pos_regresor.coef_[1]
            #return
        elif pitch < 0:
            speed_pitch = -5
            time_pitch_2_move = (np.abs(pitch) - self.el_speed_neg_regresor.coef_[0]*speed_pitch - self.el_speed_neg_regresor.intercept_)/self.el_speed_neg_regresor.coef_[1]
        elif pitch == 0:
            time_pitch_2_move = 0

    elif model == 'gp':
        if yaw == 0:
            yaw = None
        else:
            yaw = float(yaw)
        if pitch == 0:
            pitch = None
        else:
            pitch = float(pitch)

        speed_yaw, time_yaw_2_move, speed_pitch, time_pitch_2_move = self.setPosControlGPModel(yaw=yaw, pitch=yaw)

    if (time_yaw_2_move > 0) and (time_pitch_2_move > 0):
        print("[DEBUG]: Gremsy H16 moves in yaw first: TIME to complete movement: ", time_yaw_2_move)
        self.sbus.move_gimbal(0, speed_yaw, time_yaw_2_move)
        print("[DEBUG]: Gremsy H16 moves in elevation second: TIME to complete movement: ", time_pitch_2_move)
        self.sbus.move_gimbal(speed_pitch, 0, time_pitch_2_move)
    elif (time_yaw_2_move > 0) and (time_pitch_2_move <= 0):
        print("[DEBUG]: Gremsy H16 moves in yaw: TIME to complete movement: ", time_yaw_2_move)
        self.sbus.move_gimbal(0, speed_yaw, time_yaw_2_move)
    elif (time_yaw_2_move <= 0) and (time_pitch_2_move > 0):
        print("[DEBUG]: Gremsy H16 moves in elevation: TIME to complete movement: ", time_pitch_2_move)
        self.sbus.move_gimbal(speed_pitch, 0, time_pitch_2_move)
    elif (time_yaw_2_move <= 0) and (time_pitch_2_move <= 0):
        print("[DEBUG]: Gremsy H16 will not move")
        return

setPosControlGPModel(yaw=0, pitch=0)

Finds the RC control value and time giving the desired yaw (or pitch, or both separately) for the gaussian regressor.

Uses an iterative approach to find the RC control value and time to hold it by smartly searching in the grid composed by the RC control values and the times to hold it. The initial value of for the "speed" (RC control value) influences the convergence of the iterative method.

this function requires:

1. a better grid of measured "speed", time, angle. With the actual training samples, the prediction does not give physical consistent results (i.e. the time it takes to move 60 degrees at speed 20 is smaller than the one it takes to move 60 degrees at a lower speed). This is because the grid is coarse and not equally sampled.
2. this function should guarantee that the returned time is always positive. Negative times does not make physical sense. Furthermore, it should also guarantee that the time is above a certain threshold (i.e. 2 seconds), during which the gimbal will accelerate until reaching the desired speed. In a realistic gimbal, the acceleration of the gimbal is not infinite. On the contrary, gimbal's speed vs time dependence follows usually a trapezoidal curve, which means that there is some time required (the threshold) for the gimbal to reach the plateau of the trapezoid (desired speed).
3. the caller of this function (setPosControl) to handle when an exeception is raised.

Parameters:

Name	Type	Description	Default
yaw	int	desired yaw angle to set in the interval [-180,180]. Defaults to 0.	0
pitch	int	desired pitch angle to set. Defaults to 0.	0

Raises:

Type	Description
Exception	when the optimization local function find_feature_values_for_angle does not converge.

Returns:

Name	Type	Description
speed_yaw	float	required RC control value to set yaw.
time_yaw	float	required time to set yaw.
speed_pitch	float	required RC control value to set pitch
time_pitch	float	required time to set pitch.

Source code in a2gmeasurements.py

def setPosControlGPModel(self, yaw=0, pitch=0):
    """
    Finds the RC control value and time giving the desired yaw (or pitch, or both separately) for the gaussian regressor. 

    Uses an iterative approach to find the RC control value and time to hold it by smartly searching in the grid composed by the RC control values and the times to hold it. The initial value of for the "speed" (RC control value) influences the convergence of the iterative method.

    NOTE FOR DEVELOPERS: *this function requires*:
     1. a better grid of measured "speed", time, angle. With the actual training samples, the prediction does not give physical consistent results (i.e. the time it takes to move 60 degrees at speed 20 is smaller than the one it takes to move 60 degrees at a lower speed). This is because the grid is coarse and not equally sampled.
     2. this function should guarantee that the returned time is always positive. Negative times does not make physical sense. Furthermore, it should also guarantee that the time is above a certain threshold (i.e. 2 seconds), during which the gimbal will accelerate until reaching the desired speed. In a realistic gimbal, the acceleration of the gimbal is not infinite. On the contrary, gimbal's speed vs time dependence follows usually a trapezoidal curve, which means that there is some time required (the threshold) for the gimbal to reach the plateau of the trapezoid (desired speed).
     3. the caller of this function (``setPosControl``) to handle when an exeception is raised.

    Args:
        yaw (int, optional): desired yaw angle to set in the interval [-180,180]. Defaults to 0.
        pitch (int, optional): desired pitch angle to set. Defaults to 0.

    Raises:
        Exception: when the optimization local function ``find_feature_values_for_angle`` does not converge.

    Returns:
        speed_yaw (float): required RC control value to set yaw.
        time_yaw (float): required time to set yaw. 
        speed_pitch (float): required RC control value to set pitch
        time_pitch (float): required time to set pitch.
    """

    start_time = time.time()

    # Define a function to find the corresponding X values for the desired Y
    def find_feature_values_for_angle(model, desired_angle, suggested_speed):           
        # Define a function to minimize (difference between predicted and desired Y)
        def objective_function(x):
            #x = np.atleast_2d(x)
            y = np.array([[suggested_speed, x]])
            return np.abs(model.predict(y) - desired_angle)

        # Initialize with a guess for speed and time
        initial_guess = 3

        # Use an optimization method to find the feature values that result in the desired yaw angle
        result = minimize_scalar(objective_function, bounds=(0, 50), method='bounded')
        #result = minimize(objective_function, initial_guess, method='Nelder-Mead')

        if result.success:
            return result.x
        else:
            raise Exception("Optimization did not converge.")

    total_time = time.time() - start_time

    # Find the corresponding feature values (speed and time) for the desired angle
    if yaw > 0.0 and yaw < 10:
        speed_yaw = 10.0
        time_yaw = find_feature_values_for_angle(self.az_speed_pos_regresor, yaw, speed_yaw)
    elif yaw >= 10.0 and yaw < 60:
        speed_yaw = 13.0
        time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
    elif yaw >= 60:
        speed_yaw = 15.0
        time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
    if yaw < 0.0 and yaw > -10:
        speed_yaw = -3.0
        time_yaw = find_feature_values_for_angle(self.az_speed_pos_regresor, yaw, speed_yaw)
    elif yaw <= -10.0 and yaw > -60:
        speed_yaw = -7.0
        time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
    elif yaw <= -60:
        speed_yaw = -10.0
        time_yaw = find_feature_values_for_angle(self.az_speed_neg_regresor, yaw, speed_yaw)
    elif yaw == 0.0:
        speed_yaw = 0
        time_yaw = 0
    if pitch > 0.0 and pitch < 10.0:
        speed_pitch = 5.0
        time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
    elif pitch >=10 and pitch < 60:
        speed_pitch = 8.0
        time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
    elif pitch >= 60:
        speed_pitch = 10.0
        time_pitch = find_feature_values_for_angle(self.el_speed_pos_regresor, pitch, speed_pitch)
    elif pitch < 0.0 and pitch > -10:
        speed_pitch = -5.0
        time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
    elif pitch <-10 and pitch > -60:
        speed_pitch = -8.0
        time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
    elif pitch <= -60:
        speed_pitch = -10.0
        time_pitch = find_feature_values_for_angle(self.el_speed_neg_regresor, pitch, speed_pitch)
    elif pitch == 0.0:
        speed_pitch = 0
        time_pitch = 0

    #print(f"[DEBUG]: Time it took to find the speed and time corresponding to the DESIRED ANGLE: {total_time}")

    return speed_yaw, time_yaw, speed_pitch, time_pitch

start_imu_thread(COM_PORT='COM21')

Connects to the IMU and creates a new thread (the imu thread) to read the angle data from the IMU.

:param COM_PORT: , defaults to 'COM21' :type COM_PORT: str, optional

Parameters:

Name	Type	Description	Default
COM_PORT	str	port where the IMU is connected. If this host computer's OS is Windows it would be COM#, if it is Linux it would be /dev/ttyUSB#. Defaults to COM21.	'COM21'

Source code in a2gmeasurements.py

def start_imu_thread(self, COM_PORT='COM21'):
    """
    Connects to the IMU and creates a new thread (the imu thread) to read the angle data from the IMU.

    :param COM_PORT: , defaults to 'COM21'
    :type COM_PORT: str, optional

    Args:
        COM_PORT (str, optional): port where the IMU is connected. If this host computer's OS is Windows it would be ``COM#``, if it is Linux it would be ``/dev/ttyUSB#``. Defaults to ``COM21``.
    """

    try:
        self.imu_serial = serial.Serial(COM_PORT, 9600)
        print("[DEBUG]: Connected to IMU")
    except Exception as e:
        print("[DEBUG]: Exception when connecting to IMU: ", e)
    else:
        try:
            self.event_stop_thread_imu = threading.Event()                
            self.thread_read_imu = threading.Thread(target=self.receive_imu_data, args=(self.event_stop_thread_imu,))
            self.thread_read_imu.start()
            print(f"[DEBUG]: READ IMU THREAD STARTED")
        except Exception as e:
            print("[DEBUG]: Exception when starting READ IMU thread")

start_thread_gimbal()

Creates an instance of the SBUSEncoder class responsible for encoding the actual serial signal used by sbus protocol to set an RC control value.

By creating such instance, the gremsy gimbal thread is created and started.

Source code in a2gmeasurements.py

def start_thread_gimbal(self):
    """
    Creates an instance of the ``SBUSEncoder`` class responsible for encoding the actual serial signal used by sbus protocol to set an RC control value.

    By creating such instance, the gremsy gimbal thread is created and started.
    """

    self.sbus = SBUSEncoder()
    self.sbus.start_sbus(serial_interface='COM20', period_packet=0.015)

    self.define_home_position()

stop_thread_gimbal()

Stops the imu thread and the SBUSEncoder gremsy gimbal thread.

Source code in a2gmeasurements.py

def stop_thread_gimbal(self):
    """
    Stops the imu thread and the ``SBUSEncoder`` gremsy gimbal thread.
    """
    self.stop_thread_imu()
    self.sbus.stop_updating()

stop_thread_imu()

Stops the imu thread and closed the serial port where the IMU is connected.

Source code in a2gmeasurements.py

def stop_thread_imu(self):
    """
    Stops the imu thread and closed the serial port where the IMU is connected.
    """

    if self.thread_read_imu.is_alive():
        self.event_stop_thread_imu.set()

    self.imu_serial.close()