# 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  
In implementing custom changes to ArduSub, it is important that modifications are precise and that the rest of the codebase's functionality is preserved. The locations in the code handling individual thruster allocation were identified to be within the files SIM_Submarine.cpp and AP_Motors6 DOF.cpp. The former carries out calculations for state updates of the SITL virtual vehicle due to motor inputs and contains a matrix \[\widehat{\mathbf{T}}= \begin{pmatrix}-1&-1&1&1&0&0&0&0\\ 1&-1&1&-1&0&0&0&0\\ 0&0&0&0&1&1&1&1\\ 0&0&0&0&1&-1&1&-1\\ 0&0&0&0&-1&-1&1&1\\ 1&-1&-1&1&0&0&0&0\end{pmatrix} . \tag{7}\]  
In this equation, a hat is used to distinguish \(\widehat{\mathbf{T}}\) from the matrix \(\mathbf{T}\). The latter contains a matrix for distributing pilot control inputs to the individual thruster activations with the form  
\[\widehat{\mathbf{T}}^{\top}= \begin{pmatrix}-1&1&0&0&0&1\\ -1&-1&0&0&0&-1\\ 1&1&0&0&0&-1\\ 1&-1&0&0&0&1\\ 0&0&1&1&-1&0\\ 0&0&1&-1&-1&0\\ 0&0&1&1&1&0\\ 0&0&1&-1&1&0\end{pmatrix} , \tag{8}\]  
where \(\widehat{\mathbf{T}}^{\top}\) is the transpose of Equation (7). Upon closer inspection of the two matrices, both are organized in the same fashion as the thruster allocation matrix and its inverse, and are used for similar purposes: \(\widehat{\mathbf{T}}\) is to \(\mathbf{T}\) as \(\widehat{\mathbf{T}}^{\top}\) is to \(\mathbf{T}^{\dagger}\). Where the zero entries in \(\widehat{\mathbf{T}}\) and \(\widehat{\mathbf{T}}^{\top}\) are the entries close to zero in \(\mathbf{T}\) and \(\mathbf{T}^{\dagger}\), the nonzero entries also share the same positive or negative sign. However, the hatted matrices lack the dimensionality that the theoretical ones possess because they do not implicitly encode physical units and were intended for a heuristic control scheme instead of a quantitative one. Figure 5 provides a simple flow chart visualization of the following description. Every iteration of the software control loop checks the RC_Input channel for the six-by-one vector of PWM signals ranging from \(1100\,\mathrm{\SIUnitSymbolMicro s}\) to \(1900\,\mathrm{\SIUnitSymbolMicro s}\), centered on \(1500\,\mathrm{\SIUnitSymbolMicro s}\) where values \(>1500\,\mathrm{\SIUnitSymbolMicro s}\) are positive values and \(<1500\,\mathrm{\SIUnitSymbolMicro s}\) are negative values. The six entries correspond to surge, sway, heave, roll, pitch, and yaw commands and are each normalized to a \(-1.0\) to \(1.0\) range that becomes the user input vector \(\mathbf{u}\). This vector \(\mathbf{u}\) is then read into AP_Motors6 DOF.cpp where the function motor_command() calculates the thruster allocation; Algorithm 1 details this loop and shall be discussed shortly. Calculations performed in the primary loop of AP_Motors6 DOF.cpp take the forces and torques requested by \(\mathbf{u}\) and determine how the eight individual thrusters should activate to serve these commands. The resulting eight-by-one vector of these calculations \(\mathbf{m}^{*}\) is analogous to the forces from Equation (5). Each entry represents one of the eight  
Figure 5: Manual control data travel through programs sequentially in arrays which are marked with arrows below their symbolic variables; above the boxes are the library names and the inside of the boxes are the module names. thrusters and is normalized to a \(-1.0\) to \(1.0\) range. The entries of \(m^{*}\) are converted once more to PWM signals in the \(1100\,\upmu\)s to \(1900\,\upmu\)s range and output as \(\widehat{m}\) to the RC_Output channel. Once in RC_Output, the PWMs are distributed throughout the respective ESCs corresponding to the eight individual thrusters. Ultimately, this activates the thrusters that generate forces and torques on the vehicle. To understand how exactly \(\widehat{T}^{\top}\) is used, Algorithm 1 provides a step-by-step clarification. As of the ArduSub 4.5.0 beta1 release on 22 February 2024, the Ardupilot/ArduSub source code still utilizes Algorithm 1 found in AP_Motors6 DOF.cpp for the manual control mode [33].