Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: allow plane and rover to use SIM_ENGINE_FAIL

This commit is contained in:
Andrew Tridgell 2017-04-08 11:50:28 +10:00
parent 0cf571d338
commit 3236090d71
1 changed files with 4 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -378,28 +378,18 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
bool motors_on = false;
if (engine_fail > 7) {
if (engine_fail >= ARRAY_SIZE(input.servos)) {
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) {
// 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;
} 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;
} else {
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
for (i=0; i<4; i++) {
// check motors do not exceed their limits