mirror of https://github.com/ArduPilot/ardupilot
HAL_SITL: allow plane and rover to use SIM_ENGINE_FAIL
This commit is contained in:
parent
0cf571d338
commit
3236090d71
|
@ -378,28 +378,18 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
|
||||||
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
|
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
|
||||||
bool motors_on = false;
|
bool motors_on = false;
|
||||||
|
|
||||||
if (engine_fail > 7) {
|
if (engine_fail >= ARRAY_SIZE(input.servos)) {
|
||||||
engine_fail = 0;
|
engine_fail = 0;
|
||||||
}
|
}
|
||||||
|
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
|
||||||
|
input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000;
|
||||||
|
|
||||||
if (_vehicle == ArduPlane) {
|
if (_vehicle == ArduPlane) {
|
||||||
// add in engine multiplier
|
|
||||||
if (input.servos[2] > 1000) {
|
|
||||||
input.servos[2] = ((input.servos[2]-1000) * engine_mul) + 1000;
|
|
||||||
if (input.servos[2] > 2000) input.servos[2] = 2000;
|
|
||||||
}
|
|
||||||
motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
|
motors_on = ((input.servos[2]-1000)/1000.0f) > 0;
|
||||||
} else if (_vehicle == APMrover2) {
|
} else if (_vehicle == APMrover2) {
|
||||||
// add in engine multiplier
|
|
||||||
if (input.servos[2] != 1500) {
|
|
||||||
input.servos[2] = ((input.servos[2]-1500) * engine_mul) + 1500;
|
|
||||||
if (input.servos[2] > 2000) input.servos[2] = 2000;
|
|
||||||
if (input.servos[2] < 1000) input.servos[2] = 1000;
|
|
||||||
}
|
|
||||||
motors_on = ((input.servos[2]-1500)/500.0f) != 0;
|
motors_on = ((input.servos[2]-1500)/500.0f) != 0;
|
||||||
} else {
|
} else {
|
||||||
motors_on = false;
|
motors_on = false;
|
||||||
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
|
|
||||||
input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000;
|
|
||||||
// run checks on each motor
|
// run checks on each motor
|
||||||
for (i=0; i<4; i++) {
|
for (i=0; i<4; i++) {
|
||||||
// check motors do not exceed their limits
|
// check motors do not exceed their limits
|
||||||
|
|
Loading…
Reference in New Issue