Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

adding tiltrotor airframe for gazebo simulation #24028

Open
wants to merge 9 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 12 additions & 10 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
Original file line number Diff line number Diff line change
Expand Up @@ -60,19 +60,21 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
# controls in practical scenarios.

# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
param set-default SIM_GZ_SV_FUNC3 203
param set-default SIM_GZ_SV_MIN3 0
param set-default SIM_GZ_SV_MAX3 1000
param set-default SIM_GZ_SV_DIS3 500
param set-default SIM_GZ_SV_FAIL3 500
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_MIN1 900
param set-default SIM_GZ_SV_MAX1 2700
param set-default SIM_GZ_SV_DIS1 1800
param set-default SIM_GZ_SV_FAIL1 1800

# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
# - on minimum when disarmed or failed:
param set-default SIM_GZ_SV_FUNC4 204
param set-default SIM_GZ_SV_MIN4 0
param set-default SIM_GZ_SV_MAX4 1000
param set-default SIM_GZ_SV_DIS4 500
param set-default SIM_GZ_SV_FAIL4 500
param set-default SIM_GZ_SV_FUNC2 202
param set-default SIM_GZ_SV_MIN2 900
param set-default SIM_GZ_SV_MAX2 2700
param set-default SIM_GZ_SV_DIS2 1800
param set-default SIM_GZ_SV_FAIL2 1800

param set-default CA_SV_CS_COUNT 2

# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,5 +47,8 @@ param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_DIS1 100

# Steering
param set-default CA_SV_CS_COUNT 1
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_REV 1
param set-default SIM_GZ_SV_MIN1 1500
param set-default SIM_GZ_SV_MAX1 2100
112 changes: 112 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor
Original file line number Diff line number Diff line change
@@ -0,0 +1,112 @@
#!/bin/sh
#
# @name VTOL Tiltrotor
#
# @type VTOL Tiltrotor
#

. ${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:=tiltrotor}

param set-default SIM_GZ_EN 1 # Gazebo bridge

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 21

param set-default CA_AIRFRAME 3

param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.1515
param set-default CA_ROTOR0_PY 0.245
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR1_PX -0.1515
param set-default CA_ROTOR1_PY -0.1875
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR2_PX 0.1515
param set-default CA_ROTOR2_PY -0.245
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.1515
param set-default CA_ROTOR3_PY 0.1875
param set-default CA_ROTOR3_KM -0.05

param set-default CA_ROTOR0_TILT 1
param set-default CA_ROTOR2_TILT 2
param set-default CA_SV_TL0_MAXA 90
param set-default CA_SV_TL0_MINA 0
param set-default CA_SV_TL0_TD 0
param set-default CA_SV_TL0_CT 1
param set-default CA_SV_TL1_MAXA 90
param set-default CA_SV_TL1_MINA 0
param set-default CA_SV_TL1_TD 0
param set-default CA_SV_TL1_CT 1

param set-default CA_SV_CS0_TRQ_R -0.5
param set-default CA_SV_CS0_TYPE 1
param set-default CA_SV_CS1_TRQ_R 0.5
param set-default CA_SV_CS1_TYPE 2
param set-default CA_SV_CS2_TRQ_P 1
param set-default CA_SV_CS2_TYPE 3
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_TL_COUNT 2

param set-default SIM_GZ_EC_FUNC1 101
param set-default SIM_GZ_EC_FUNC2 102
param set-default SIM_GZ_EC_FUNC3 103
param set-default SIM_GZ_EC_FUNC4 104

param set-default SIM_GZ_EC_MIN1 10
param set-default SIM_GZ_EC_MIN2 10
param set-default SIM_GZ_EC_MIN3 10
param set-default SIM_GZ_EC_MIN4 10

param set-default SIM_GZ_EC_MAX1 1500
param set-default SIM_GZ_EC_MAX2 1500
param set-default SIM_GZ_EC_MAX3 1500
param set-default SIM_GZ_EC_MAX4 1500

param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_FUNC2 202
param set-default SIM_GZ_SV_FUNC3 203
param set-default SIM_GZ_SV_FUNC4 204
param set-default SIM_GZ_SV_FUNC5 205

param set-default SIM_GZ_SV_MAX4 2700
param set-default SIM_GZ_SV_MIN4 1800
param set-default SIM_GZ_SV_MAX5 2700
param set-default SIM_GZ_SV_MIN5 1800

param set-default NPFG_PERIOD 12
param set-default FW_PR_FF 0.2
param set-default FW_PR_P 0.9
param set-default FW_PSP_OFF 2
param set-default FW_P_LIM_MIN -15
param set-default FW_RR_FF 0.1
param set-default FW_RR_P 0.3
param set-default FW_THR_TRIM 0.38
param set-default FW_THR_MAX 0.6
param set-default FW_THR_MIN 0.05
param set-default FW_T_CLMB_MAX 8
param set-default FW_T_SINK_MAX 2.7
param set-default FW_T_SINK_MIN 2.2

param set-default MC_AIRMODE 1
param set-default MC_YAWRATE_P 0.4
param set-default MC_YAWRATE_I 0.1

param set-default MPC_XY_VEL_P_ACC 3
param set-default MPC_XY_VEL_I_ACC 4
param set-default MPC_XY_VEL_D_ACC 0.1

param set-default MIS_TAKEOFF_ALT 10

param set-default VT_FWD_THRUST_EN 4
param set-default VT_FWD_THRUST_SC 0.6
param set-default VT_TILT_TRANS 0.6
param set-default VT_TYPE 1
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ px4_add_romfs_files(
4016_gz_x500_lidar_down
4017_gz_x500_lidar_front
4018_gz_quadtailsitter
4020_gz_tiltrotor

6011_gazebo-classic_typhoon_h480
6011_gazebo-classic_typhoon_h480.post
Expand Down
24 changes: 22 additions & 2 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include "GZMixingInterfaceServo.hpp"


bool GZMixingInterfaceServo::init(const std::string &model_name)
{
// /model/rascal_110_0/servo_2
Expand All @@ -53,6 +54,22 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
return true;
}

/**
*@brief Servo output are mapped from ms into deg.
* The outputs are uint16 and to allow for decimals the
* range is scaled by SERVO_OUTPUT_SCALING * [0, 360] deg.
* With an offset SERVO_OUTPUT_OFFSET the outputs can be mapped into negative angles.
*
* Hence,
* SIM_GZ_SV_MIN{n} = (SERVO_OUTPUT_OFFSET + myAngularOffsetDeg) * SERVO_OUTPUT_SCALING
* results with angles within [-180,180] deg.
*
* Ex. ailerons with a myAngularOffsetDeg of +-45 deg would be
* SIM_GZ_SV_MIN{n} = 1350 and SIM_GZ_SV_MAX{n} = 2250.
*
* Note: SIM_GZ_SV_MIN{n} and SIM_GZ_SV_MAX{n} must be coherent with
* joint limits in the airframe/model.sdf
*/
bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
Expand All @@ -64,8 +81,11 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA
for (auto &servo_pub : _servos_pub) {
if (_mixing_output.isFunctionSet(i)) {
gz::msgs::Double servo_output;
///TODO: Normalize output data
double output = (outputs[i] - 500) / 500.0;

// TODO: update the SIM_GZ_ params to floats instead of uint to unlock
// parameterizing of min/max angles and servo min/max outputs in ms.
double output = math::radians((double)outputs[i] / SERVO_OUTPUT_SCALING - SERVO_OUTPUT_OFFSET);

// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
// std::cout << " output: " << output << std::endl;
servo_output.set_data(output);
Expand Down
5 changes: 5 additions & 0 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,11 @@ class GZMixingInterfaceServo : public OutputModuleInterface

void Run() override;

// Scales the servo outputs into the range 360 deg
static constexpr double SERVO_OUTPUT_SCALING = 10.;
// Offsets the servo value in degrees into a range of +-180 deg
static constexpr double SERVO_OUTPUT_OFFSET = 180.;

gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;

Expand Down
8 changes: 4 additions & 4 deletions src/modules/simulation/gz_bridge/module.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,10 @@ actuator_output:
group_label: 'Servos'
channel_label: 'Servo'
standard_params:
disarmed: { min: 0, max: 1000, default: 500 }
min: { min: 0, max: 1000, default: 0 }
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
disarmed: { min: 0, max: 3600, default: 1800 }
min: { min: 0, max: 3600, default: 1350 }
max: { min: 0, max: 3600, default: 2250 }
failsafe: { min: 0, max: 3600 }
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where do these numbers come from? Can we normalize it for simulation?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The goal is to map the full rotation of -180° to 180° (probably the most you could ever want) as a uint16 range of 0 to 3600 because we (still) don't have float mixing which was drafted in #22488

Copy link
Member

@Jaeyoung-Lim Jaeyoung-Lim Dec 17, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, thanks! so this was what I was wondering. I guess it is fine for now, but would wish if we can remove the arbitrary scaling which a lot of people having stumbling over (including myself repeatedly many times).

num_channels: 8
- param_prefix: SIM_GZ_WH
group_label: 'Wheels'
Expand Down
Loading