From 36f3cae535f0153e4cc96f80c6fa599951a11365 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sun, 30 Oct 2016 08:34:41 -0700 Subject: [PATCH] SITL: Add support for Sub --- libraries/SITL/Frame_Vectored.h | 50 +++++++++++++++++ libraries/SITL/SIM_Submarine.cpp | 96 ++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Submarine.h | 62 +++++++++++++++++++++ 3 files changed, 208 insertions(+) create mode 100644 libraries/SITL/Frame_Vectored.h create mode 100644 libraries/SITL/SIM_Submarine.cpp create mode 100644 libraries/SITL/SIM_Submarine.h diff --git a/libraries/SITL/Frame_Vectored.h b/libraries/SITL/Frame_Vectored.h new file mode 100644 index 0000000000..cdb5fb2bb0 --- /dev/null +++ b/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 diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp new file mode 100644 index 0000000000..740972c14a --- /dev/null +++ b/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 . + */ +/* + Submarine simulator class +*/ + +#include "SIM_Submarine.h" +#include +#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(); +} diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h new file mode 100644 index 0000000000..49549035a1 --- /dev/null +++ b/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 . + */ +/* + 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; +}; +}