diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10045_sihsim_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/10045_sihsim_rover_ackermann new file mode 100644 index 0000000000..88f8bdd67e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10045_sihsim_rover_ackermann @@ -0,0 +1,72 @@ +#!/bin/sh +# @name Rover Ackermann +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +set VEHICLE_TYPE rover_ackermann +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 driving +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:=rover_ackermann} + +param set-default SIH_VEHICLE_TYPE 5 # sih as rover ackermann + +param set-default PWM_MAIN_FUNC1 201 # Steering +param set-default PWM_MAIN_FUNC2 101 # Throttle +param set-default SIH_MASS 20 +param set-default SIH_IXX 0.4333 +param set-default SIH_IYY 1.6833 +param set-default SIH_IZZ 2.0833 +param set-default SIH_IXZ 0 +param set-default SIH_KDV 50 +param set-default SIH_KDW 10 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 + +# 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 + +# Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 400 +param set-default RO_YAW_DECEL_LIM 800 +param set-default RO_YAW_RATE_CORR 1 + +# Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Velocity Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 3 +param set-default RO_JERK_LIM 10 +param set-default RO_MAX_THR_SPEED 3.2 +param set-default RO_SPEED_LIM 3 +param set-default RO_SPEED_I 0.001 +param set-default RO_SPEED_P 0.001 +param set-default RO_SPEED_RED 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 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index e33c888a19..b621344331 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -108,6 +108,7 @@ px4_add_romfs_files( 10042_sihsim_xvert 10043_sihsim_standard_vtol 10044_sihsim_hex + 10045_sihsim_rover_ackermann 17001_flightgear_tf-g1 17002_flightgear_tf-g2 diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 0be8db3c1b..3dc290a775 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -55,6 +55,7 @@ if(PX4_PLATFORM MATCHES "posix") xvert standard_vtol hex + rover_ackermann ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 32b973c3f8..d6227ed4fa 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -211,7 +211,7 @@ void Sih::sensor_step() read_motors(dt); - generate_force_and_torques(); + generate_force_and_torques(dt); equations_of_motion(dt); @@ -290,6 +290,7 @@ void Sih::init_variables() _lpos = Vector3f(0.0f, 0.0f, 0.0f); _v_N = Vector3f(0.0f, 0.0f, 0.0f); + _v_N_dot = Vector3f(0.0f, 0.0f, 0.0f); _p_E = Vector3d(Wgs84::equatorial_radius, 0.0, 0.0); _v_E = Vector3f(0.0f, 0.0f, 0.0f); _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); @@ -318,7 +319,7 @@ void Sih::read_motors(const float dt) } } -void Sih::generate_force_and_torques() +void Sih::generate_force_and_torques(const float dt) { if (_vehicle == VehicleType::Quadcopter) { _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); @@ -367,6 +368,9 @@ 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::RoverAckermann) { + generate_rover_ackermann_dynamics(_u[1], _u[0], dt); } } @@ -422,6 +426,73 @@ void Sih::generate_ts_aerodynamics() _Ma_B = _R_S2B * Ma_ts - _KDW * _w_B; // aerodynamic moments } +void Sih::generate_rover_ackermann_dynamics(const float throttle_cmd, const float steering_cmd, const float dt) +{ + // --- Constants --- + static constexpr float MAX_THROTTLE_FORCE = 400.0f; // [N] + static constexpr float MAX_STEER_ANGLE = radians(30.f); // [rad] + static constexpr float WHEEL_BASE = 0.321f; // [m] Distance between front and rear axle + static constexpr float C = 500.f; // [N/rad] Cornering stiffness + static constexpr float MU_S = 0.5f; // [-] Static Coefficient of friction + static constexpr float MU_K = 0.4f; // [-] Kinetic Coefficient of friction + static constexpr float MU_R = 0.3f; // [-] Rolling Coefficient of friction + static constexpr float ROLLING_THRESHOLD = 0.05f; // [m/s] Threshold for rolling resistance + static constexpr float STATIC_THRESHOLD = 0.01f; // [m/s] Threshold for static resistance + + matrix::Vector3f v_B = _q.rotateVectorInverse(_v_N); // Nav -> Body + + // --- Compute inputs --- + const float delta = MAX_STEER_ANGLE * steering_cmd; // [rad] Steering angle + const float F_x = MAX_THROTTLE_FORCE * throttle_cmd; // [N] Throttle force + + // --- Compute forces and moments --- + float F_y = 0.f; // [N] Lateral force + float M_z = 0.f; // [Nm] Yaw moment + + if (fabsf(v_B(0)) > ROLLING_THRESHOLD) { + // Equations based on the lateral dynamics of the bicycle model from [1] + // [1] Sri Anumakonda, Everything you need to know about Self-Driving Cars in <30 minutes + // Link: https://srianumakonda.medium.com/everything-you-need-to-know-about-self-driving-in-30-minutes-b38d68bd3427 + const float a_y = C * delta / _MASS - fabsf(v_B(0)) * _w_B(2) + - 2 * C * v_B(1) / (_MASS * fabsf(v_B(0))); // [m/s^2] Lateral acceleration + const float psi_dot_dot = WHEEL_BASE * C * delta / _sih_izz.get() + - C * WHEEL_BASE * WHEEL_BASE * _w_B(2) / (_sih_izz.get() * fabsf(v_B(0))); // [rad/s^2] Yaw acceleration + F_y = _MASS * a_y; // Lateral force [N] + M_z = _sih_izz.get() * psi_dot_dot; // [Nm] Yaw moment + } + + _T_B = Vector3f(F_x, F_y, 0.f); + _Mt_B = Vector3f(0.f, 0.f, M_z); + + // --- Compute drag/friction forces and moments --- + Vector3f F_f = Vector3f(0.f, 0.f, 0.f); // [N] Friction force + Vector3f F_a = Vector3f(0.f, 0.f, 0.f); // [N] Aerodynamic force (neglect until rover is rolling) + + if (_v_E.norm() < STATIC_THRESHOLD) { // Static friction + Vector3f F_f_B = Vector3f(sign(F_x) * math::min(fabsf(F_x), MU_S * _MASS * 9.81f), sign(F_y) * math::min(fabsf(F_y), + MU_S * _MASS * 9.81f), 0.f); + F_f = _q_E.rotateVector(F_f_B); + + } else if (_v_E.norm() < ROLLING_THRESHOLD) { // Kinetic friction + if (_T_B.norm() > FLT_EPSILON) { + F_f = _v_E.unit_or_zero() * MU_K * _MASS * 9.81f; + + } else { + F_f = _v_E * _MASS / dt; // Stop the vehicle + } + + } else { // Rolling friction + F_f = _v_E.unit_or_zero() * MU_R * _MASS * 9.81f; + Vector3f v_E_squared = Vector3f(sign(_v_E(0)) * _v_E(0) * _v_E(0), sign(_v_E(1)) * _v_E(1) * _v_E(1), + sign(_v_E(2)) * _v_E(2) * _v_E(2)); + F_a = _KDV * v_E_squared; // [N] Second order drag + } + + _Fa_E = -F_a - F_f; // [N] Second order drag and friction + _Ma_B = -_KDW * _w_B; // [Nm] First order angular damper + +} + float Sih::computeGravity(const double lat) { // Somigliana formula for gravitational acceleration @@ -459,7 +530,8 @@ void Sih::equations_of_motion(const float dt) _grounded = true; - } else if (_vehicle == VehicleType::FixedWing) { + } else if (_vehicle == VehicleType::FixedWing + || _vehicle == VehicleType::RoverAckermann) { Vector3f down_u = _R_N2E.col(2); ground_force_E = -down_u * sum_of_forces_E * down_u; @@ -749,6 +821,9 @@ int Sih::print_status() } else if (_vehicle == VehicleType::StandardVTOL) { PX4_INFO("Standard VTOL"); + + } else if (_vehicle == VehicleType::RoverAckermann) { + PX4_INFO("Rover Ackermann"); } 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 4bcc5a281a..bda4484884 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -151,7 +151,7 @@ private: void read_motors(const float dt); // generate the motors thrust and torque in the body frame - void generate_force_and_torques(); + void generate_force_and_torques(const float dt); // apply the equations of motion of a rigid body and integrate one step void equations_of_motion(const float dt); @@ -163,6 +163,7 @@ private: void publish_ground_truth(const hrt_abstime &time_now_us); void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); + void generate_rover_ackermann_dynamics(const float throttle_cmd, const float steering_cmd, const float dt); void sensor_step(); static float computeGravity(double lat); @@ -220,7 +221,7 @@ private: float _u[NUM_ACTUATORS_MAX] {}; // thruster signals - enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, First = Quadcopter, Last = Hexacopter}; // numbering dependent on parameter SIH_VEHICLE_TYPE + enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, RoverAckermann, First = Quadcopter, Last = RoverAckermann}; // numbering dependent on parameter SIH_VEHICLE_TYPE VehicleType _vehicle = VehicleType::Quadcopter; // aerodynamic segments for the fixedwing diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index c3112ca598..25c9498e63 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -334,6 +334,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 2 Tailsitter * @value 3 Standard VTOL * @value 4 Hexacopter + * @value 5 Rover Ackermann * @reboot_required true * @group Simulation In Hardware */