From 9cd0aef53e715f2e742b2dc4996cba82f952945b Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 5 Aug 2019 13:44:46 -0300 Subject: [PATCH] SITL: Add 6dof frame for Sub --- Tools/autotest/default_params/sub-6dof.parm | 62 +++++++++++++++++++++ Tools/autotest/pysim/vehicleinfo.py | 4 ++ libraries/AP_HAL_SITL/SITL_cmdline.cpp | 1 + libraries/SITL/SIM_Submarine.cpp | 25 ++++++++- libraries/SITL/SIM_Submarine.h | 28 +++++----- 5 files changed, 105 insertions(+), 15 deletions(-) create mode 100644 Tools/autotest/default_params/sub-6dof.parm diff --git a/Tools/autotest/default_params/sub-6dof.parm b/Tools/autotest/default_params/sub-6dof.parm new file mode 100644 index 0000000000..67c7af365f --- /dev/null +++ b/Tools/autotest/default_params/sub-6dof.parm @@ -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 \ No newline at end of file diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index 6ee59c3b9b..83d52f9574 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -264,6 +264,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", diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 6670e27495..b99001d29f 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -133,6 +133,7 @@ static const struct { { "calibration", Calibration::create }, { "vectored", Submarine::create }, { "silentwings", SilentWings::create }, + { "vectored_6dof", Submarine::create }, { "morse", Morse::create }, { "airsim", AirSim::create}, { "scrimmage", Scrimmage::create }, diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index deacbc80b6..5b1bca6758 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -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 *frame_str) : @@ -41,6 +53,15 @@ Submarine::Submarine(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) { diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h index a82aafb843..245e7ae373 100644 --- a/libraries/SITL/SIM_Submarine.h +++ b/libraries/SITL/SIM_Submarine.h @@ -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 *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; }; }