# Modifications to ArduSub That Improve BlueROV SITL Accuracy and Design of Hybrid Autopilot
## 2 ArduSub Alterations
### Changes to ArduSub and Motivation
#### 2.2.2 Implementation
However, the exact value of the resulting forces/torques, \(\mathbf{\tau}\), will not directly correlate to the magnitude of the input commands, which is necessary for automatic control laws. Instead, quantification of forces should be preserved, and saturation avoided through selection of an adequate control bandwidth. Thus, with this assessment of the original control implementation and the goal of implementing a PD control law, a new simple algorithm is determined which can replace the pre-existing framework without disrupting anything else or introducing unnecessary complexity. In order for the new manual control algorithm to be integrated seamlessly, without altering processes on either end of the software control loop, it must use the same inputs and produce the same outputs. The main challenge is incorporating physical units that will allow the autopilot to command specific quantities of forces and torques. To achieve this, \(\mathbf{T}^{\mathbf{t}}\) replaces \(\widehat{\mathbf{T}}^{\top}\) and new parameters and functions are introduced that encode the BROV2 hardware limitations while preserving the control architecture. These new parameters are \(\mathbf{\tau}_{max}\), \(\mathbf{PWM}^{*}\), and \(\mathbf{t}\), which are a six-by-one vector containing the maximum forces and torques that the BROV2 is capable of, eighty-one normalized PWMs and eighty-one corresponding thrusts produced by a T200 thruster. The contents of \(\mathbf{PWM}^{*}\) and \(\mathbf{t}\) are experimental data collected by Blue Robotics [34] and graphed in Figure 6 and used as a lookup table, utilized by the FMU thrust distribution and the SITL hydrodynamic model. The values of \(\mathbf{\tau}_{max}\) are determined analytically by solving for \(\mathbf{\tau}\) using Equation (1) given \(\mathbf{m}\), constructed of maximum thrusts according to the lookup table in combinations producing solely surge, sway, heave, roll, pitch, or yaw manoeuvrers. This results in \(\mathbf{\tau}_{max,13}=[141.29,141.29,199.81]\) N for forces and \(\mathbf{\tau}_{max,46}=[43.56,23.98,37.72]\) Nm for torques. To use input signals from RC_Input of \(\mathbf{u}\in[-1.0,1.0]\), the signals are converted to units of forces and torques with \(\mathbf{\tau}_{max}\), and transformed with \(\mathbf{T}^{\mathbf{t}}\) to find \(\mathbf{m}\) directly. Mathematically, these operations are  
\[\mathbf{m}=\mathbf{T}^{\mathbf{t}}\mathbf{diag}(\mathbf{\tau}_{max})\mathbf{u}. \tag{9}\]  
Algorithm 2 shows how Equation (9) is accomplished in ArduSub step by step. Using the new parameters \(\mathbf{\tau}_{max}\), \(\mathbf{PWM}^{*}\), and \(\mathbf{t}\), the same inputs \(\mathbf{u}\) are used and the same outputs \(\widehat{\mathbf{m}}\) are produced. However, now \(\mathbf{u}\) originates from a hybrid autopilot which has calculated physical forces and torques (that have been normalized with respect to \(\mathbf{\tau}_{max}\)), as opposed to interpreting inputs heuristically. Linear DOF control is handled with an open-loop controller that directly passes inputs into \(\mathbf{u}_{13}\). Angular control is handled with a closed-loop PD control law that interprets an AR pilot's gestures with error feedback, before passing inputs into \(\mathbf{u}_{46}\). Section 4 shall discuss the control law in detail. The new function motor_command_new() uses one software loop; first, it iteratively performs Equation (9) (line 3) to determine \(m_{i}\). This is then input into the function find_bounding_indicies(), which determines the closest indices of \(\mathbf{t}\) below (\({}_{low}\)) and above (\({}_{high}\)) the force of \(m_{i}\) (line 4). In the event that \(m_{i}\) is outside the range of \(\mathbf{t}\), the \({}_{low}\)index will equal the \(\mathit{n}_{\mathit{high}}\) index and default to either \(-1.0\) for negative forces or \(1.0\) for positive forces (lines 5 through 12). For all other instances, the bounding indices will be used to perform linear interpolation that produces a normalized value of \(m_{i}^{*}\) (lines 13 through 15).