diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp new file mode 100644 index 0000000000..acc50af09e --- /dev/null +++ b/libraries/SITL/SIM_Submarine.cpp @@ -0,0 +1,72 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 + +using namespace SITL; + +Submarine::Submarine(const char *home_str, const char *frame_str) : + Aircraft(home_str, frame_str), + frame(NULL) +{ + frame = Frame::find_frame(frame_str); + if (frame == NULL) { + printf("Frame '%s' not found", frame_str); + exit(1); + } + if (strstr(frame_str, "-fast")) { + frame->init(1.5, 0.5, 85, 4*radians(360)); + } else { + frame->init(1.5, 0.51, 15, 4*radians(360)); + } + frame_height = 0.1; + ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; +} + +// calculate rotational and linear accelerations +void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) +{ + frame->calculate_forces(*this, input, rot_accel, body_accel); +} + +/* + 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..8937a185b9 --- /dev/null +++ b/libraries/SITL/SIM_Submarine.h @@ -0,0 +1,49 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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; +}; + +}