diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_boat new file mode 100644 index 000000000000..91bf3770a2bc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_boat @@ -0,0 +1,81 @@ +#!/bin/sh +# @name Boat (Ackermann style) +# @type Boat +# @class Boat + +. ${R}etc/init.d/rc.boat_defaults + +set VEHICLE_TYPE boat +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 + + +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=boat} + +param set-default SIH_VEHICLE_TYPE 4 # sih as boat + +#param set-default SIH_MASS 3.0 +#param set-default SIH_IXX 0.00402 +#param set-default SIH_IYY 0.0144 +#param set-default SIH_IZZ 0.0177 +#param set-default SIH_IXZ 0.00046 +#param set-default SIH_KDV 0.2 + +param set-default PWM_MAIN_FUNC1 201 # Steering +param set-default PWM_MAIN_FUNC2 101 # Throttle + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +#param set-default SENS_EN_ARSPDSIM 1 + +param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 +param set-default RA_ACC_RAD_GAIN 2 +param set-default RA_ACC_RAD_MAX 3 +param set-default RA_MAX_STR_ANG 0.5236 +param set-default RA_STR_RATE_LIM 360 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 15 +param set-default RO_MAX_THR_SPEED 3.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 1 +param set-default RO_YAW_RATE_LIM 180 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 3 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 3 +param set-default RO_SPEED_I 0.1 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 + +# Wheels +param set-default SIM_GZ_WH_FUNC1 101 +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 + +# Steering +param set-default SIM_GZ_SV_MAXA1 30 +param set-default SIM_GZ_SV_MINA1 -30 +param set-default CA_SV_CS_COUNT 1 +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_REV 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index b4e107799fd0..395ff09e08c0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -109,6 +109,7 @@ px4_add_romfs_files( 10041_sihsim_airplane 10042_sihsim_xvert 10043_sihsim_standard_vtol + 10044_sihsim_boat 17001_flightgear_tf-g1 17002_flightgear_tf-g2 diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 7c6f8f43699c..08eb588b7956 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -73,8 +73,13 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( rc.rover_apps rc.rover_defaults + ) +endif() - rc.boat_defaults # hack +if(CONFIG_SIH_SIM_BOAT) + px4_add_romfs_files( + rc.boat_apps + rc.boat_defaults ) endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_apps b/ROMFS/px4fmu_common/init.d/rc.boat_apps new file mode 100644 index 000000000000..dd59d8824696 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_apps @@ -0,0 +1,8 @@ +#!/bin/sh +# Standard apps for an boat. + +# Start rover ackermann module. +rover_ackermann start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index ae34645ad890..3189dd15c192 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -49,6 +49,14 @@ then # Start ackermann drive rover apps. . ${R}etc/init.d/rc.rover_ackermann_apps fi +# +# Boat setup. +# +if [ $VEHICLE_TYPE = boat ] +then + # Start boat apps. + . ${R}etc/init.d/rc.boat_apps +fi # # Mecanum Rover setup. diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f85c03e23a2a..1879957fca24 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -49,6 +49,8 @@ CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_MECANUM=y CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_SIH_SIM_BOAT=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_MSGS=y diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index 9bbd356d9843..41368823a54f 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -9,7 +9,7 @@ It may or may not work with current versions of PX4 (known to work in PX4 v1.14) See [Toolchain Installation](../dev_setup/dev_env.md) for information about the environments and tools supported by the core development team. ::: -Simulation-In-Hardware (SIH) is an alternative to [Hardware In The Loop simulation (HITL)](../simulation/hitl.md) for quadrotors, fixed-wing vehicles (airplane), and VTOL tailsitters. +Simulation-In-Hardware (SIH) is an alternative to [Hardware In The Loop simulation (HITL)](../simulation/hitl.md) for quadrotors, fixed-wing vehicles (airplane), VTOL tailsitters and boats. SIH can be used by new PX4 users to get familiar with PX4 and the different modes and features, and of course to learn to fly a vehicle using an RC controller in simulation, which is not possible using SITL. @@ -27,6 +27,7 @@ The Desktop computer is only used to display the virtual vehicle. - SIH for fixed-wing (airplane) and VTOL tailsitter are supported from PX4 v1.13. - SIH as SITL (without hardware) from PX4 v1.14. - SIH for Standard VTOL from PX4 v1.16. +- SIH for boats from PX4 v1.16. ### Benefits @@ -61,6 +62,7 @@ To set up/start SIH: - [SIH plane AERT](../airframes/airframe_reference.md#plane_simulation_sih_plane_aert) - [SIH Tailsitter Duo](../airframes/airframe_reference.md#vtol_simulation_sih_tailsitter_duo) - [SIH Standard VTOL QuadPlane](../airframes/airframe_reference.md#vtol_simulation_sih_standard_vtol_quadplane) + - Boat uses [Rover Ackermann](../airframes/airframe_reference.md#rover_rover_generic_rover_ackermann) as CA model The autopilot will then reboot. The `sih` module is started on reboot, and the vehicle should be displayed on the ground control station map. @@ -132,12 +134,18 @@ To run SIH as SITL: make px4_sitl sihsim_xvert ``` - - Standard VTOL: + - Standard VTOL: ```sh make px4_sitl sihsim_standard_vtol ``` + - Boat: + + ```sh + make px4_sitl sihsim_boat + ``` + ### Change Simulation Speed SITL allows the simulation to be run faster than real time. diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 7ae8483c5186..20464cf0985f 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -54,6 +54,7 @@ if(PX4_PLATFORM MATCHES "posix") quadx xvert standard_vtol + boat ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/Kconfig b/src/modules/simulation/simulator_sih/Kconfig index 6f3be6f63712..a6782ea90905 100644 --- a/src/modules/simulation/simulator_sih/Kconfig +++ b/src/modules/simulation/simulator_sih/Kconfig @@ -8,6 +8,15 @@ menuconfig MODULES_SIMULATION_SIMULATOR_SIH ---help--- Enable support for simulator_sih +if MODULES_SIMULATION_SIMULATOR_SIH + config SIH_SIM_BOAT + bool "Enable Boat Simulation features (SIH)" + default n + depends on MODULES_SIMULATION_SIMULATOR_SIH + ---help--- + Enable boat support for SIH simulator. +endif # MODULES_SIMULATION_SIMULATOR_SIH + menuconfig USER_SIH bool "simulator_sih running as userspace module" default y diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 30ac027d6f55..6af247f9197b 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -76,7 +76,7 @@ void Sih::run() _airspeed_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(3)); + static_cast(5)); _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; @@ -351,9 +351,66 @@ void Sih::generate_force_and_torques() // thrust 0 because it is already contained in _T_B. in // equations_of_motion they are all summed into sum_of_forces_E generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); + + } else if (_vehicle == VehicleType::Boat) { + // _T_B = Vector3f(_T_MAX * _u[1], 0.0f, 0.0f); // forward thruster + // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque + // _Mt_B = Vector3f(); + // _Mt_B = Vector3f(0.f, 0.f, _Q_MAX * _u[3] * (+_u[0])); + generate_boat_dynamics(_u[1], _u[0]); + + _Fa_E = -_KDV * _v_E; // first order drag to slow down the vehicle + _Ma_B = -_KDW * _w_B; // first order angular damper + } } +void Sih::generate_boat_dynamics(const float throttle_cmd, const float steering_cmd) +{ + const float MAX_FORCE = 100.0f; // [N] + const float MAX_STEER_ANGLE = 1.2f; // [rad] (=68.7°) + const float MOMENT_EFFECTIVENESS = 0.08f; // unitless (0..1) + const float ENGINE_TO_COG = 1.0f; // [m] + + const float DAMPING_X = 2.0f; // forward drag [N·s/m] + const float DAMPING_Y = 8.0f; // lateral drag [N·s/m] + const float YAW_DAMPING = 1.0f; // yaw damping [Nm·s/rad] + const float LATERAL_RESISTANCE = 20.0f; // keel-like resistance [N·s/m] + + // --- Convert nav velocity to body frame --- + const matrix::Dcmf R_nb(_q); // body to nav frame + const matrix::Vector3f v_B = R_nb.T() * _v_N; // nav -> body + + // --- Compute input-based thrust --- + const float steering_angle = MAX_STEER_ANGLE * steering_cmd; + const float force = MAX_FORCE * throttle_cmd; + + _T_B = matrix::Vector3f( + std::cos(steering_angle) * force, + std::sin(steering_angle) * force, + 0.0f + ); + + // --- Compute yaw moment from steering (scaled by speed) --- + const float speed = v_B.norm(); + const float steer_eff = math::constrain(speed / 10.0f, 0.0f, 1.0f); // more effective at higher speeds + + _Mt_B = matrix::Vector3f( + 0.0f, + 0.0f, + std::sin(steering_angle) * ENGINE_TO_COG * force * MOMENT_EFFECTIVENESS * steer_eff + ); + + // --- Apply drag damping --- + _T_B(0) -= DAMPING_X * v_B(0); // forward resistance + _T_B(1) -= DAMPING_Y * v_B(1); // lateral resistance + _T_B(1) -= LATERAL_RESISTANCE * v_B(1); // extra "keel" effect + + // --- Angular damping (if you have _w_B) --- + _Mt_B(2) -= YAW_DAMPING * _w_B(2); // oppose yaw rotation +} + + void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float throttle_cmd) { @@ -452,6 +509,18 @@ void Sih::equations_of_motion(const float dt) ground_force_E += down_u * (-_v_N(2) / dt) * _MASS; } + _grounded = true; + + } else if (_vehicle == VehicleType::Boat) { + Vector3f down_u = _R_N2E.col(2); + ground_force_E = -down_u * sum_of_forces_E * down_u; + + if (!_grounded) { + // if we just hit the floor + // compute the force that will stop the vehicle in one time step + ground_force_E += down_u * (-_v_N(2) / dt) * _MASS; + } + _grounded = true; } @@ -729,6 +798,10 @@ int Sih::print_status() } else if (_vehicle == VehicleType::StandardVTOL) { PX4_INFO("Running Standard VTOL"); + + } else if (_vehicle == VehicleType::Boat) { + PX4_INFO("Running Boat"); + } PX4_INFO("vehicle landed: %d", _grounded); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 557ec2b16c19..646ef55b7613 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -161,6 +161,7 @@ class Sih : public ModuleBase, public ModuleParams void send_airspeed(const hrt_abstime &time_now_us); void send_dist_snsr(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); + void generate_boat_dynamics(const float roll_cmd, const float throttle_cmd); void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); void sensor_step(); @@ -220,7 +221,7 @@ class Sih : public ModuleBase, public ModuleParams float _u[NUM_ACTUATORS_MAX] {}; // thruster signals - enum class VehicleType {Multicopter, FixedWing, TailsitterVTOL, StandardVTOL}; + enum class VehicleType {Multicopter, FixedWing, TailsitterVTOL, StandardVTOL, Boat}; VehicleType _vehicle = VehicleType::Multicopter; diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index f9e053a5ebea..6615880b28f1 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -333,6 +333,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 1 Fixed-Wing * @value 2 Tailsitter * @value 3 Standard VTOL + * @value 4 Boat * @reboot_required true * @group Simulation In Hardware */