aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: Add SITL Submarine files

Browse Source
This commit is contained in:
Rustom Jehangir 2016-10-30 08:34:41 -07:00
parent 4febcfe288
commit 29f8ce9535
2 changed files with 121 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/SITL/SIM_Submarine.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
Submarine simulator class
*/
#include "SIM_Submarine.h"
#include <AP_Motors/AP_Motors.h>
#include <stdio.h>
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();
}

49
libraries/SITL/SIM_Submarine.h Normal file
View File

@ -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 <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;
};
}