# 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
The matrix is used as a constant parameter that helps transform user input \(u\) into motor activation \(m^{*}\). The function performing the computation is motor_command(). Within the function, there are various forms of \(\xi\) which serve as buffer variables used inside the function. The first usage is introduced on lines 2 and 9, with the form \({}^{(j)}\xi_{max}\), where the left superscript \(j\) can be either 1 for roll, pitch, and heave control or 2 for yaw, surge, and sway control and the right subscript \"max\" labels its purpose as a normalizing variable. The second type of usage is introduced on lines 4 and 11, with the form \({}^{(j)}\xi_{i}\), where the left superscript \(j\) again can be either 1 for roll, pitch, and heave control or 2 for yaw, surge, and sway control and the right subscript \(i\) is an index indicating the \(i\)-th thruster which is being calculated for. The groupings of forces in \({}^{(1)}\xi\) terms correspond to those generated primarily by the vertical thrusters and groupings of forces in \({}^{(2)}\xi\) correspond to those generated primarily by the azimuthal thrusters. ```
Parameters:\(\widehat{T}^{\top}_{8\times 6}\) Input:\(u_{6\times 1}\) Output:\(m^{*}_{8\times 1}\)
1 Functionmotor_command(\(u,\widehat{T}^{\top}\)):
2\({}^{(1)}\xi_{max}=1\);
3 for\(i\gets 1\)to 8 do
4\({}^{(1)}\xi_{i}=u_{3}\cdot\widehat{T}^{\top}_{i,3}+u_{4:5}\cdot\widehat{T}^{ \top}_{i,4:5}\);
5 if\(|{}^{(1)}\xi_{i}|>^{(1)}\xi_{max}\)then
6\({}^{(1)}\xi_{max}=^{(1)}\xi_{i}\)
7 end if
8
9 end for
10\({}^{(2)}\xi_{max}=1\);
11 for\(i\gets 1\)to 8 do
12\({}^{(2)}\xi_{i}=u_{1:2}\cdot\widehat{T}^{\top}_{i,1:2}+u_{6}\cdot\widehat{T}^{ \top}_{i,6}\);
13 if\(|{}^{(2)}\xi_{i}|>^{(2)}\xi_{max}\)then
14\({}^{(2)}\xi_{max}=^{(2)}\xi_{i}\)
15 end if
16
17 end for
18 for\(i\gets 1\)to 8 do
19\(m^{*}_{i}=\frac{(1)}\xi_{i}\) + \(\frac{(2)}\xi_{i}\) ;
20\(\text{constrain}(m_{i}\in[-1.0,1.0])\);
21\(\widehat{m}_{i}=1500+400m^{*}_{i}\);
22
23 end for
24 send \(\widehat{m}\) to RC_Output channels;
```  
**Algorithm 1**Original Manual Control Algorithm  
The function motor_command() consists of three loops, two for calculating preliminary output values based on inputs and the third loop for constructing the output. The first loop and second loop are identical except that the first loop calculates for roll, pitch, and heave control and the second loop calculates for yaw, surge, and sway control. The first two loopsstep through the thrusters in order and scale outputs with respect to the corresponding parts of \(\widehat{\mathbf{T}}^{\top}\) with respect to the corresponding user inputs of \(\mathbf{u}\) and sum them together in the buffer \({}^{(j)}\xi_{i}\) (lines 4 and 11). These numbers are then checked against the current value of \({}^{(j)}\xi_{max}\) and replaced with the value of the \({}^{(j)}\xi_{i}\) buffer if it is larger (lines 5 and 12). After all eight thrusters have been looped over twice, the final third loop normalizes \({}^{(j)}\xi_{i}\) with respect to the final values of \({}^{(j)}\xi_{max}\) and adds the vertical and azimuthal ratios together (line 17). The trimming function constrain() checks that each value of \(\mathbf{m}^{*}\) is in the range of \(-1.0\) to \(1.0\) (line 18); if a value is outside the bounds, the function chooses the limit closest. Then, the values of \(\mathbf{m}^{*}\) are scaled up to appropriate PWMs (line 19), and finally sent to RC_Output which relays the signals to the respective ESCs. If a user were to command any combination of movement, the vehicle would respond with all requested movements, but all would be scaled with respect to the largest commanded force or torque. The overall effect of Algorithm 1 is that it generates intuitive vehicle responses that a pilot will recognize as matching joystick inputs. This heuristic method is a good way to prevent cases of motor saturation where the vehicle appears to be unresponsive due to motors being maxed out.