|
|
@ -236,6 +236,12 @@ SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char * |
|
|
|
} |
|
|
|
} |
|
|
|
benewake_tfmini = new SITL::RF_Benewake_TFmini(); |
|
|
|
benewake_tfmini = new SITL::RF_Benewake_TFmini(); |
|
|
|
return benewake_tfmini; |
|
|
|
return benewake_tfmini; |
|
|
|
|
|
|
|
} else if (streq(name, "teraranger_serial")) { |
|
|
|
|
|
|
|
if (teraranger_serial != nullptr) { |
|
|
|
|
|
|
|
AP_HAL::panic("Only one teraranger_serial at a time"); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
teraranger_serial = new SITL::RF_TeraRanger_Serial(); |
|
|
|
|
|
|
|
return teraranger_serial; |
|
|
|
} else if (streq(name, "lightwareserial")) { |
|
|
|
} else if (streq(name, "lightwareserial")) { |
|
|
|
if (lightwareserial != nullptr) { |
|
|
|
if (lightwareserial != nullptr) { |
|
|
|
AP_HAL::panic("Only one lightwareserial at a time"); |
|
|
|
AP_HAL::panic("Only one lightwareserial at a time"); |
|
|
@ -567,6 +573,9 @@ void SITL_State::_fdm_input_local(void) |
|
|
|
if (benewake_tfmini != nullptr) { |
|
|
|
if (benewake_tfmini != nullptr) { |
|
|
|
benewake_tfmini->update(sitl_model->rangefinder_range()); |
|
|
|
benewake_tfmini->update(sitl_model->rangefinder_range()); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (teraranger_serial != nullptr) { |
|
|
|
|
|
|
|
teraranger_serial->update(sitl_model->rangefinder_range()); |
|
|
|
|
|
|
|
} |
|
|
|
if (lightwareserial != nullptr) { |
|
|
|
if (lightwareserial != nullptr) { |
|
|
|
lightwareserial->update(sitl_model->rangefinder_range()); |
|
|
|
lightwareserial->update(sitl_model->rangefinder_range()); |
|
|
|
} |
|
|
|
} |
|
|
|