SITL: Add 6dof frame for Sub

Tools/autotest/default_params/sub-6dof.parm

@@ -0,0 +1,62 @@
+BARO_RND         0.02
+BATT_MONITOR     4
+BTN0_FUNCTION    0
+BTN10_SFUNCTION  0
+BTN10_FUNCTION   22
+BTN10_SFUNCTION  0
+BTN11_FUNCTION   42
+BTN11_SFUNCTION  47
+BTN12_FUNCTION   43
+BTN12_SFUNCTION  46
+BTN13_FUNCTION   33
+BTN13_SFUNCTION  45
+BTN14_FUNCTION   32
+BTN14_SFUNCTION  44
+BTN15_FUNCTION   0
+BTN15_SFUNCTION  0
+BTN1_FUNCTION    6
+BTN1_SFUNCTION   0
+BTN2_FUNCTION    8
+BTN2_SFUNCTION   0
+BTN3_FUNCTION    7
+BTN3_SFUNCTION   0
+BTN4_FUNCTION    4
+BTN4_SFUNCTION   0
+BTN5_FUNCTION    1
+BTN5_SFUNCTION   0
+BTN6_FUNCTION    3
+BTN6_SFUNCTION   0
+BTN7_FUNCTION    21
+BTN7_SFUNCTION   0
+BTN8_FUNCTION    48
+BTN8_SFUNCTION   0
+BTN9_FUNCTION    23
+BTN9_SFUNCTION   0
+COMPASS_OFS_X   5
+COMPASS_OFS_Y   13
+COMPASS_OFS_Z   -18
+COMPASS_OFS2_X   5
+COMPASS_OFS2_Y   13
+COMPASS_OFS2_Z   -18
+COMPASS_OFS3_X   5
+COMPASS_OFS3_Y   13
+COMPASS_OFS3_Z   -18
+INS_ACCOFFS_X   0.001
+INS_ACCOFFS_Y   0.001
+INS_ACCOFFS_Z   0.001
+INS_ACCSCAL_X   1.001
+INS_ACCSCAL_Y   1.001
+INS_ACCSCAL_Z   1.001
+INS_ACC2OFFS_X   0.001
+INS_ACC2OFFS_Y   0.001
+INS_ACC2OFFS_Z   0.001
+INS_ACC2SCAL_X   1.001
+INS_ACC2SCAL_Y   1.001
+INS_ACC2SCAL_Z   1.001
+INS_ACC3OFFS_X   0.000
+INS_ACC3OFFS_Y   0.000
+INS_ACC3OFFS_Z   0.000
+INS_ACC3SCAL_X   1.000
+INS_ACC3SCAL_Y   1.000
+INS_ACC3SCAL_Z   1.000
+FRAME_CONFIG     2.000

Tools/autotest/pysim/vehicleinfo.py

@@ -264,6 +264,10 @@ def __init__(self):
                 "waf_target": "bin/ardusub",
                 "default_params_filename": "default_params/sub.parm",
             },
+            "vectored_6dof": {
+                "waf_target": "bin/ardusub",
+                "default_params_filename": "default_params/sub-6dof.parm",
+            },
             "gazebo-bluerov2": {
                 "waf_target": "bin/ardusub",
                 "default_params_filename": "default_params/sub.parm",

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -134,6 +134,7 @@ static const struct {
     { "plane",              Plane::create },
     { "calibration",        Calibration::create },
     { "vectored",           Submarine::create },
+    { "vectored_6dof",      Submarine::create },
     { "silentwings",        SilentWings::create },
     { "morse",              Morse::create },
     { "airsim",             AirSim::create},

libraries/SITL/SIM_Submarine.cpp

@@ -33,6 +33,18 @@ static Thruster vectored_thrusters[] =
        Thruster(5,          -1.0f,          0,              0,              -1.0f,              0,                  0)
 };
 
+
+static Thruster vectored_6dof_thrusters[] =
+{
+       //       Motor #     Roll Factor     Pitch Factor    Yaw Factor      Throttle Factor     Forward Factor      Lateral Factor
+       Thruster(0,          0,              0,              1.0f,           0,                  -1.0f,              1.0f),
+       Thruster(1,          0,              0,              -1.0f,          0,                  -1.0f,              -1.0f),
+       Thruster(2,          0,              0,              -1.0f,          0,                  1.0f,               1.0f),
+       Thruster(3,          0,              0,              1.0f,           0,                  1.0f,               -1.0f),
+       Thruster(4,          1.0f,           -1.0f,          0,              -1.0f,              0,                  0),
+       Thruster(5,          -1.0f,          -1.0f,          0,              -1.0f,              0,                  0),
+       Thruster(6,          1.0f,           1.0f,           0,              -1.0f,              0,                  0),
+       Thruster(7,          -1.0f,          1.0f,           0,              -1.0f,              0,                  0)
 };
 
 Submarine::Submarine(const char *frame_str) :
@@ -41,6 +53,15 @@ Submarine::Submarine(const char *frame_str) :
 {
     frame_height = 0.0;
     ground_behavior = GROUND_BEHAVIOR_NONE;
+
+    // default to vectored frame
+    thrusters = vectored_thrusters;
+    n_thrusters = 6;
+
+    if (strstr(frame_str, "vectored_6dof")) {
+        thrusters = vectored_6dof_thrusters;
+        n_thrusters = 8;
+    }
 }
 
 // calculate rotational and linear accelerations
@@ -51,8 +72,8 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
     // slight positive buoyancy
     body_accel = Vector3f(0, 0, -calculate_buoyancy_acceleration());
 
-    for (int i = 0; i < 6; i++) {
-        Thruster t = vectored_thrusters[i];
+    for (int i = 0; i < n_thrusters; i++) {
+        Thruster t = thrusters[i];
         int16_t pwm = input.servos[t.servo];
         float output = 0;
         if (pwm < 2000 && pwm > 1000) {

libraries/SITL/SIM_Submarine.h

@@ -29,6 +29,19 @@ namespace SITL {
  */
 
 
+class Thruster {
+public:
+    Thruster(int8_t _servo, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac) :
+        servo(_servo)
+    {
+        linear = Vector3f(forward_fac, lat_fac, -throttle_fac);
+        rotational = Vector3f(roll_fac, pitch_fac, yaw_fac);
+    };
+    int8_t servo;
+    Vector3f linear;
+    Vector3f rotational;
+};
+
 class Submarine : public Aircraft {
 public:
     Submarine(const char *frame_str);
@@ -80,18 +93,7 @@ class Submarine : public Aircraft {
     void calculate_drag_force(const Vector3f &velocity, const Vector3f &drag_coefficient, Vector3f &force);
 
     Frame *frame;
-};
-
-class Thruster {
-public:
-    Thruster(int8_t _servo, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac) :
-        servo(_servo)
-    {
-        linear = Vector3f(forward_fac, lat_fac, -throttle_fac);
-        rotational = Vector3f(roll_fac, pitch_fac, yaw_fac);
-    };
-    int8_t servo;
-    Vector3f linear;
-    Vector3f rotational;
+    Thruster* thrusters;
+    uint8_t n_thrusters;
 };
 }