SITL: Add support for Sub

libraries/SITL/Frame_Vectored.h

@@ -0,0 +1,50 @@
+#pragma once
+
+using namespace SITL;
+
+#define MOT_1_ROLL_FACTOR 0.0
+#define MOT_1_PITCH_FACTOR 0.0
+#define MOT_1_YAW_FACTOR 1.0
+#define MOT_1_THROTTLE_FACTOR 0.0
+#define MOT_1_FORWARD_FACTOR -1
+#define MOT_1_STRAFE_FACTOR 1
+
+//Front left
+#define MOT_2_ROLL_FACTOR 0.0
+#define MOT_2_PITCH_FACTOR 0.0
+#define MOT_2_YAW_FACTOR -1.0
+#define MOT_2_THROTTLE_FACTOR 0.0
+#define MOT_2_FORWARD_FACTOR -1
+#define MOT_2_STRAFE_FACTOR -1
+
+//Back right
+#define MOT_3_ROLL_FACTOR 0
+#define MOT_3_PITCH_FACTOR 0
+#define MOT_3_YAW_FACTOR -1.0
+#define MOT_3_THROTTLE_FACTOR 0.0
+#define MOT_3_FORWARD_FACTOR 1
+#define MOT_3_STRAFE_FACTOR 1
+
+//Back left
+#define MOT_4_ROLL_FACTOR 0
+#define MOT_4_PITCH_FACTOR 0
+#define MOT_4_YAW_FACTOR 1.0
+#define MOT_4_THROTTLE_FACTOR 0.0
+#define MOT_4_FORWARD_FACTOR 1
+#define MOT_4_STRAFE_FACTOR -1
+
+//Right facing up
+#define MOT_5_ROLL_FACTOR 1.0
+#define MOT_5_PITCH_FACTOR 0.0
+#define MOT_5_YAW_FACTOR 0.0
+#define MOT_5_THROTTLE_FACTOR -1
+#define MOT_5_FORWARD_FACTOR 0.0
+#define MOT_5_STRAFE_FACTOR 0.0
+
+//Left facing up
+#define MOT_6_ROLL_FACTOR -1.0
+#define MOT_6_PITCH_FACTOR 0.0
+#define MOT_6_YAW_FACTOR 0.0
+#define MOT_6_THROTTLE_FACTOR -1
+#define MOT_6_FORWARD_FACTOR 0.0
+#define MOT_6_STRAFE_FACTOR 0.0

libraries/SITL/SIM_Submarine.cpp

@@ -0,0 +1,96 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Submarine simulator class
+*/
+
+#include "SIM_Submarine.h"
+#include <AP_Motors/AP_Motors.h>
+#include "Frame_Vectored.h"
+
+using namespace SITL;
+
+static Thruster vectored_thrusters[] =
+{
+       Thruster(0, MOT_1_ROLL_FACTOR, MOT_1_PITCH_FACTOR, MOT_1_YAW_FACTOR, MOT_1_THROTTLE_FACTOR, MOT_1_FORWARD_FACTOR, MOT_1_STRAFE_FACTOR),
+       Thruster(1, MOT_2_ROLL_FACTOR, MOT_2_PITCH_FACTOR, MOT_2_YAW_FACTOR, MOT_2_THROTTLE_FACTOR, MOT_2_FORWARD_FACTOR, MOT_2_STRAFE_FACTOR),
+       Thruster(2, MOT_3_ROLL_FACTOR, MOT_3_PITCH_FACTOR, MOT_3_YAW_FACTOR, MOT_3_THROTTLE_FACTOR, MOT_3_FORWARD_FACTOR, MOT_3_STRAFE_FACTOR),
+       Thruster(3, MOT_4_ROLL_FACTOR, MOT_4_PITCH_FACTOR, MOT_4_YAW_FACTOR, MOT_4_THROTTLE_FACTOR, MOT_4_FORWARD_FACTOR, MOT_4_STRAFE_FACTOR),
+       Thruster(4, MOT_5_ROLL_FACTOR, MOT_5_PITCH_FACTOR, MOT_5_YAW_FACTOR, MOT_5_THROTTLE_FACTOR, MOT_5_FORWARD_FACTOR, MOT_5_STRAFE_FACTOR),
+       Thruster(5, MOT_6_ROLL_FACTOR, MOT_6_PITCH_FACTOR, MOT_6_YAW_FACTOR, MOT_6_THROTTLE_FACTOR, MOT_6_FORWARD_FACTOR, MOT_6_STRAFE_FACTOR)
+
+};
+
+Submarine::Submarine(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str),
+    frame(NULL)
+{
+    frame_height = 0.1;
+    ground_behavior = GROUND_BEHAVIOR_NONE;
+}
+
+// calculate rotational and linear accelerations
+void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
+{
+    rot_accel = Vector3f(0,0,0);
+    body_accel = Vector3f(0,0,0);
+    for (int i = 0; i < 6; i++) {
+        Thruster t = vectored_thrusters[i];
+        int16_t pwm = input.servos[t.servo];
+        float output = 0;
+        if (pwm < 2000 && pwm > 1000) {
+         output = (pwm - 1500) / 400.0f; // range -1~1
+        }
+
+        body_accel += t.linear * output * 1.2 * GRAVITY_MSS;
+        rot_accel += t.rotational * output;
+    }
+
+    float terminal_rotation_rate = 10.0;
+    if (terminal_rotation_rate > 0) {
+        // rotational air resistance
+        rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
+        rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
+        rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
+    }
+
+    float terminal_velocity = 3.0;
+    if (terminal_velocity > 0) {
+        // air resistance
+        Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
+        body_accel += dcm.transposed() * air_resistance;
+    }
+}
+
+/*
+  update the Submarine simulation by one time step
+ */
+void Submarine::update(const struct sitl_input &input)
+{
+    // get wind vector setup
+    update_wind(input);
+
+    Vector3f rot_accel;
+
+    calculate_forces(input, rot_accel, accel_body);
+
+    update_dynamics(rot_accel);
+
+    // update lat/lon/altitude
+    update_position();
+
+    // update magnetic field
+    update_mag_field_bf();
+}

libraries/SITL/SIM_Submarine.h

@@ -0,0 +1,62 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  ROV/AUV/Submarine simulator class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+#include "SIM_Motor.h"
+#include "SIM_Frame.h"
+
+namespace SITL {
+
+/*
+  a submarine simulator
+ */
+
+
+class Submarine : public Aircraft {
+public:
+    Submarine(const char *home_str, const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input);
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) {
+        return new Submarine(home_str, frame_str);
+    }
+
+protected:
+    // calculate rotational and linear accelerations
+    void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
+    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;
+};
+}