From dbaca50b1371a5c5126b57a878c6b8f94e6388fe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 23 Jan 2025 10:20:53 +1100 Subject: [PATCH] AP_HAL: SIMState: do a single nullptr check in SIMState servo sim mirrors if c1016ae52ee4de3181b2c0a07369e51f05eb46f7 --- libraries/AP_HAL/SIMState.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 6c3ea8235aedb7..295016dff3f84a 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -274,6 +274,10 @@ void SIMState::fdm_input_local(void) */ void SIMState::_simulator_servos(struct sitl_input &input) { + if (_sitl == nullptr) { + return; + } + // output at chosen framerate uint32_t now = AP_HAL::micros(); @@ -288,7 +292,7 @@ void SIMState::_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. wind_speed = _sitl->wind_speed_active = (0.95f*_sitl->wind_speed_active) + (0.05f*_sitl->wind_speed); wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction); @@ -328,7 +332,7 @@ void SIMState::_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 @@ -360,8 +364,8 @@ void SIMState::_simulator_servos(struct sitl_input &input) float voltage = 0; _current = 0; - - if (_sitl != nullptr) { + + { if (_sitl->state.battery_voltage <= 0) { } else { // FDM provides voltage and current