From 7e45f4915208b2d02e05e5741ff7456cf92120f9 Mon Sep 17 00:00:00 2001 From: jmackay2 <1.732mackay@gmail.com> Date: Mon, 19 Aug 2024 02:54:57 -0400 Subject: [PATCH] Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (#23455) * Update GZBridge to be able to use gazebo airspeed. Add gz quadtailsitter. * Fix formatting --------- Co-authored-by: jmackay2 --- .../1045_gazebo-classic_quadtailsitter | 2 +- .../airframes/4004_gz_standard_vtol | 2 - .../airframes/4014_gz_quadtailsitter | 92 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + src/modules/simulation/gz_bridge/GZBridge.cpp | 16 ++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 4 +- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 2 +- 7 files changed, 103 insertions(+), 16 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 87afb523b1a7..9539ba6d7402 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 -parm set-default FD_FAIL_R 70 +param set-default FD_FAIL_R 70 param set-default FW_P_TC 0.6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 96bb25d69a52..580b6da368de 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -14,9 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 1 # TODO: Enable motor failure detection when the # VTOL no longer reports 0A for all ESCs in SITL diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter new file mode 100644 index 000000000000..699d1dacc521 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter @@ -0,0 +1,92 @@ +#!/bin/sh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Quad Tailsitter +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +param set-default MAV_TYPE 20 + +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.23 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.23 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.23 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.23 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_MIN4 10 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default FD_FAIL_R 70 + +param set-default FW_P_TC 0.6 + +param set-default FW_PR_I 0.3 +param set-default FW_PR_P 0.5 +param set-default FW_PSP_OFF 2 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_I 0.1 +param set-default FW_RR_P 0.2 +param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P +param set-default FW_YR_I 0 +param set-default FW_THR_TRIM 0.35 +param set-default FW_THR_MAX 0.8 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 6 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 3 +param set-default FW_T_SINK_MIN 1.6 +param set-default FW_AIRSPD_STALL 10 +param set-default FW_AIRSPD_MIN 14 +param set-default FW_AIRSPD_TRIM 18 +param set-default FW_AIRSPD_MAX 22 + +param set-default MC_AIRMODE 2 +param set-default MAN_ARM_GESTURE 0 # required for yaw airmode +param set-default MC_ROLL_P 3 +param set-default MC_PITCH_P 3 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_PITCHRATE_P 0.3 + +param set-default VT_ARSP_TRANS 15 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_FW_DIFTHR_EN 7 +param set-default VT_FW_DIFTHR_S_Y 1 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9235b2e66340..badd712b063f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -85,6 +85,7 @@ px4_add_romfs_files( 4011_gz_lawnmower 4012_gz_rover_ackermann 4013_gz_x500_lidar + 4014_gz_quadtailsitter 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3464c628694c..6b4042da8bb3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -203,17 +203,15 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str()); } -#if 0 - // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed - std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; + // Airspeed: /world/$WORLD/model/$MODEL/link/base_link/sensor/airspeed_sensor/air_speed + std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/airspeed_sensor/air_speed"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); return PX4_ERROR; } -#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -426,8 +424,7 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -452,7 +449,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) pthread_mutex_unlock(&_node_mutex); } -#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8a832f7961b2..824c7d471633 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -102,7 +102,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void clockCallback(const gz::msgs::Clock &clock); - // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + void airspeedCallback(const gz::msgs::AirSpeed &air_speed); void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); @@ -123,7 +123,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 7b0d9556cadf..86fbbf29cd4c 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -195,7 +195,7 @@ int SensorAirspeedSim::print_usage(const char *reason) )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system"); + PRINT_MODULE_USAGE_NAME("sensor_airspeed_sim", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();