diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 4850e40b02..06fe08506f 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -400,17 +400,18 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } float engine_mul = _sitl?_sitl->engine_mul.get():1; - uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0; + uint32_t engine_fail = _sitl?_sitl->engine_fail.get():0; float throttle = 0.0f; - if (engine_fail >= ARRAY_SIZE(input.servos)) { - engine_fail = 0; - } // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter - if (_vehicle != Rover) { - input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000; - } else { - input.servos[engine_fail] = static_cast(((input.servos[engine_fail] - 1500) * engine_mul) + 1500); + for (uint8_t i=0; i(((input.servos[i] - 1500) * engine_mul) + 1500); + } + } } if (_vehicle == ArduPlane) { diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 03ba5f1771..b4541b822d 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -246,7 +246,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Param: ENGINE_FAIL // @DisplayName: Engine Fail Mask // @Description: mask of motors which SIM_ENGINE_MUL will be applied to - // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8 + // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0), AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm), AP_SUBGROUPEXTENSION("", 60, SIM, var_mag), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 54d9350437..d80f641c5f 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -197,7 +197,7 @@ public: AP_Float drift_speed; // degrees/second/minute AP_Float drift_time; // period in minutes AP_Float engine_mul; // engine multiplier - AP_Int8 engine_fail; // engine servo to fail (0-7) + AP_Int32 engine_fail; // mask of engine/motor servo outputs to fail // initial offset on GPS lat/lon, used to shift origin AP_Float gps_init_lat_ofs;