diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index 7c49f738e8..1bbcc7d23d 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -85,6 +85,14 @@ void RCOutput::push(void) memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS * sizeof(uint16_t)); _corked = false; } + + // do not overwrite FETTec simulation's ESC telemetry data: + SITL::SITL *sitl = AP::sitl(); + if (sitl != nullptr && + sitl->fetteconewireesc_sim.enabled()) { + return; + } + if (esc_telem == nullptr) { esc_telem = new AP_ESC_Telem_SITL; } diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 347f495c96..fc64d65a85 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -367,6 +367,9 @@ int SITL_State::sim_fd(const char *name, const char *arg) } else if (streq(name, "richenpower")) { sitl_model->set_richenpower(&_sitl->richenpower_sim); return _sitl->richenpower_sim.fd(); + } else if (streq(name, "fetteconewireesc")) { + sitl_model->set_fetteconewireesc(&_sitl->fetteconewireesc_sim); + return _sitl->fetteconewireesc_sim.fd(); } else if (streq(name, "ie24")) { sitl_model->set_ie24(&_sitl->ie24_sim); return _sitl->ie24_sim.fd(); @@ -490,6 +493,8 @@ int SITL_State::sim_fd_write(const char *name) return sf45b->write_fd(); } else if (streq(name, "richenpower")) { return _sitl->richenpower_sim.write_fd(); + } else if (streq(name, "fetteconewireesc")) { + return _sitl->fetteconewireesc_sim.write_fd(); } else if (streq(name, "ie24")) { return _sitl->ie24_sim.write_fd(); } else if (streq(name, "gyus42v2")) { @@ -814,6 +819,23 @@ 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; iengine_mul.get():1; uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0; float throttle = 0.0f; diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index b6e2e9165a..a0f55bebc3 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -54,6 +54,7 @@ #include #include +#include #include class HAL_SITL; @@ -298,6 +299,9 @@ private: // simulated RPLidarA2: SITL::PS_RPLidarA2 *rplidara2; + // simulated FETtec OneWire ESCs: + SITL::FETtecOneWireESC *fetteconewireesc; + // simulated SF45B proximity sensor: SITL::PS_LightWare_SF45B *sf45b;