diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 736e7091e8..96e3516d8f 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -327,7 +327,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input) // give 5 seconds to calibrate airspeed sensor at 0 wind speed if (wind_start_delay_micros == 0) { wind_start_delay_micros = now; - } else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) { + } else if ((now - wind_start_delay_micros) > 5000000 ) { // The EKF does not like step inputs so this LPF keeps it happy. uint32_t dt_us = now - last_wind_update_us; if (dt_us > 1000) { @@ -379,27 +379,22 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } } - if (_sitl != nullptr) { - // FETtec ESC simulation support. Input signals of 1000-2000 - // are positive thrust, 0 to 1000 are negative thrust. Deeper - // changes required to support negative thrust - potentially - // adding a field to input. - if (_sitl != nullptr) { - if (_sitl->fetteconewireesc_sim.enabled()) { - _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input); - for (uint8_t i=0; ifetteconewireesc_sim.enabled()) { + _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input); + for (uint8_t i=0; iengine_mul.get():1; - uint32_t engine_fail = _sitl?_sitl->engine_fail.get():0; - float throttle = 0.0f; - + const float engine_mul = _sitl->engine_mul.get(); + const uint32_t engine_fail = _sitl->engine_fail.get(); + // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter for (uint8_t i=0; ithrottle = throttle; - } + _sitl->throttle = throttle; update_voltage_current(input, throttle); }