diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 848288ab08..bb3875e8bc 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -90,6 +90,7 @@ void SITL_State::_sitl_setup(const char *home_str) _update_ins(0); _update_compass(); _update_gps(0, 0, 0, 0, 0, 0, false); + _update_rangefinder(0); #endif if (enable_gimbal) { gimbal = new SITL::Gimbal(_sitl->state); @@ -167,6 +168,7 @@ void SITL_State::_fdm_input_step(void) !_sitl->gps_disable); _update_ins(_sitl->state.airspeed); _update_compass(); + _update_rangefinder(_sitl->state.range); if (_sitl->adsb_plane_count >= 0 && adsb == nullptr) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index c67bc8b079..fde7bdd9f8 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -84,7 +84,7 @@ private: void set_height_agl(void); void _update_compass(void); - + void _update_rangefinder(float range_value); void _set_signal_handlers(void) const; struct gps_data { diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index 6574585fe5..f5baf7c52a 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -78,49 +78,6 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed) return airspeed_raw/4; } - -/* - emulate an analog rangefinder - */ -uint16_t SITL_State::_ground_sonar(void) -{ - float altitude = _sitl->height_agl; - - // sensor position offset in body frame - Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset; - - // adjust altitude for position of the sensor on the vehicle if position offset is non-zero - if (!relPosSensorBF.is_zero()) { - // get a rotation matrix following DCM conventions (body to earth) - Matrix3f rotmat; - _sitl->state.quaternion.rotation_matrix(rotmat); - - // rotate the offset into earth frame - Vector3f relPosSensorEF = rotmat * relPosSensorBF; - // correct the altitude at the sensor - altitude -= relPosSensorEF.z; - } - - float voltage = 5.0f; - if (fabs(_sitl->state.rollDeg) < 90 && - fabs(_sitl->state.pitchDeg) < 90) { - // adjust for apparent altitude with roll - altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg)); - - altitude += _sitl->sonar_noise * rand_float(); - - // Altitude in in m, scaler in meters/volt - voltage = altitude / _sitl->sonar_scale; - voltage = constrain_float(voltage, 0, 5.0f); - - if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) { - voltage = 5.0f; - } - } - - return 1023*(voltage / 5.0f); -} - /* setup the INS input channels with new input */ @@ -131,7 +88,6 @@ void SITL_State::_update_ins(float airspeed) return; } - sonar_pin_value = _ground_sonar(); float airspeed_simulated = (fabsf(_sitl->arspd_fail) > 1.0e-6f) ? _sitl->arspd_fail : airspeed; airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * rand_float())); } diff --git a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp new file mode 100644 index 0000000000..e5e7c1817b --- /dev/null +++ b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp @@ -0,0 +1,67 @@ +/* + SITL handling + + This simulates a rangefinder + + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "AP_HAL_SITL.h" +#include "AP_HAL_SITL_Namespace.h" +#include "HAL_SITL_Class.h" +#include "SITL_State.h" +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace HALSITL; + +// TODO Improve that to not use analog +/* + setup the rangefinder with new input + */ +void SITL_State::_update_rangefinder(float range_value) +{ + float altitude = range_value; + if (range_value == -1) { + altitude = _sitl->height_agl; + } + + // sensor position offset in body frame + Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset; + + // adjust altitude for position of the sensor on the vehicle if position offset is non-zero + if (!relPosSensorBF.is_zero()) { + // get a rotation matrix following DCM conventions (body to earth) + Matrix3f rotmat; + _sitl->state.quaternion.rotation_matrix(rotmat); + // rotate the offset into earth frame + Vector3f relPosSensorEF = rotmat * relPosSensorBF; + // correct the altitude at the sensor + altitude -= relPosSensorEF.z; + } + + float voltage = 5.0f; + if (fabs(_sitl->state.rollDeg) < 90 && + fabs(_sitl->state.pitchDeg) < 90) { + // adjust for apparent altitude with roll + altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg)); + + altitude += _sitl->sonar_noise * rand_float(); + + // Altitude in in m, scaler in meters/volt + voltage = altitude / _sitl->sonar_scale; + voltage = constrain_float(voltage, 0, 5.0f); + + if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) { + voltage = 5.0f; + } + } + + sonar_pin_value = 1023*(voltage / 5.0f); +} + +#endif diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index b7d608dc4b..13c6afd12a 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -161,6 +161,7 @@ void Aircraft::update_position(void) // we only advance time if it hasn't been advanced already by the // backend + // TODO : make this a function if (last_time_us == time_now_us) { time_now_us += frame_time_us; } @@ -362,6 +363,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.rpm1 = rpm1; fdm.rpm2 = rpm2; fdm.rcin_chan_count = rcin_chan_count; + fdm.range = range; memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float)); fdm.bodyMagField = mag_bf; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 92dbebe2de..212a230300 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -139,6 +139,7 @@ protected: float rpm2 = 0; uint8_t rcin_chan_count = 0; float rcin[8]; + float range = -1; // rangefinder detection in m // Wind Turbulence simulated Data float turbulence_azimuth = 0; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 648b87ab89..27742c5736 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -25,6 +25,7 @@ struct sitl_fdm { double rpm2; // secondary RPM uint8_t rcin_chan_count; float rcin[8]; // RC input 0..1 + double range; // rangefinder value Vector3f bodyMagField; // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss. Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes };