diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 13828d49f5..ae20f0c989 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -390,7 +390,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.velocity_air_bf = velocity_air_bf; fdm.battery_voltage = battery_voltage; fdm.battery_current = battery_current; - fdm.motor_mask = motor_mask; + fdm.motor_mask = motor_mask | sitl->vibe_motor_mask; memcpy(fdm.rpm, rpm, sizeof(fdm.rpm)); fdm.rcin_chan_count = rcin_chan_count; fdm.range = rangefinder_range(); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 9689d1e92d..0ca255e9fe 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -174,6 +174,9 @@ const AP_Param::GroupInfo SIM::var_info2[] = { // motor harmonics AP_GROUPINFO("VIB_MOT_HMNC", 60, SIM, vibe_motor_harmonics, 1), + // motor mask, allowing external simulators to mark motors + AP_GROUPINFO("VIB_MOT_MASK", 5, SIM, vibe_motor_mask, 0), + // max motor vibration frequency AP_GROUPINFO("VIB_MOT_MAX", 61, SIM, vibe_motor, 0.0f), // minimum throttle for simulated ins noise diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index cacc89875b..d201d9046b 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -336,6 +336,9 @@ public: // what harmonics to generate AP_Int16 vibe_motor_harmonics; + // what servos are motors + AP_Int32 vibe_motor_mask; + // minimum throttle for addition of ins noise AP_Float ins_noise_throttle_min;