Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: Add 6dof frame for Sub

This commit is contained in:
Willian Galvani 2019-08-05 13:44:46 -03:00 committed by Jacob Walser
parent 3356baed53
commit 357293fb2c
5 changed files with 105 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
Tools/autotest/default_params/sub-6dof.parm Normal file
View File

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

4
Tools/autotest/pysim/vehicleinfo.py
View File

@ -241,6 +241,10 @@ class VehicleInfo(object):
"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",

1
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -117,6 +117,7 @@ static const struct {
{ "plane", Plane::create },
{ "calibration", Calibration::create },
{ "vectored", Submarine::create },
{ "vectored_6dof", Submarine::create },
{ "morse", Morse::create },
};

25
libraries/SITL/SIM_Submarine.cpp
View File

@ -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 *home_str, const char *frame_str) :
@ -41,6 +53,15 @@ Submarine::Submarine(const char *home_str, 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) {

28
libraries/SITL/SIM_Submarine.h
View File

@ -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 *home_str, const char *frame_str);
@ -80,18 +93,7 @@ protected:
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;
};
}