SITL: make SIM_ENGINE_FAIL a mask

The param docs already claimed it was one.
This commit is contained in:
Bob Long 2024-12-17 12:44:34 +11:00 committed by Andrew Tridgell
parent 7c084e9955
commit 58c3854325
3 changed files with 11 additions and 10 deletions
libraries

View File

@ -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<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
if (engine_fail & (1<<i)) {
if (_vehicle != Rover) {
input.servos[i] = ((input.servos[i]-1000) * engine_mul) + 1000;
} else {
input.servos[i] = static_cast<uint16_t>(((input.servos[i] - 1500) * engine_mul) + 1500);
}
}
}
if (_vehicle == ArduPlane) {

View File

@ -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),

View File

@ -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;