mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_SITL: rangefinder don't adjust value from FDM on external data
This commit is contained in:
parent
599e3d7b83
commit
4acf4538c7
|
@ -19,14 +19,13 @@ extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
using namespace HALSITL;
|
using namespace HALSITL;
|
||||||
|
|
||||||
// TODO Improve that to not use analog
|
|
||||||
/*
|
/*
|
||||||
setup the rangefinder with new input
|
setup the rangefinder with new input
|
||||||
*/
|
*/
|
||||||
void SITL_State::_update_rangefinder(float range_value)
|
void SITL_State::_update_rangefinder(float range_value)
|
||||||
{
|
{
|
||||||
float altitude = range_value;
|
float altitude = range_value;
|
||||||
if (range_value == -1) {
|
if (is_equal(range_value, -1.0f)) { // Use SITL altitude as reading by default
|
||||||
altitude = _sitl->height_agl;
|
altitude = _sitl->height_agl;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -44,24 +43,29 @@ void SITL_State::_update_rangefinder(float range_value)
|
||||||
altitude -= relPosSensorEF.z;
|
altitude -= relPosSensorEF.z;
|
||||||
}
|
}
|
||||||
|
|
||||||
float voltage = 5.0f;
|
float voltage = 5.0f; // Start the reading at max value = 5V
|
||||||
if (fabs(_sitl->state.rollDeg) < 90 &&
|
// If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
|
||||||
fabs(_sitl->state.pitchDeg) < 90) {
|
// We adjust the reading with noise, glitch and scaler as the reading is on analog port.
|
||||||
// adjust for apparent altitude with roll
|
if ((fabs(_sitl->state.rollDeg) < 90.0 && fabs(_sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) {
|
||||||
altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));
|
if (is_equal(range_value, -1.0f)) { // disable for external reading that already handle this
|
||||||
|
// adjust for apparent altitude with roll
|
||||||
|
altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));
|
||||||
|
}
|
||||||
|
// Add some noise on reading
|
||||||
altitude += _sitl->sonar_noise * rand_float();
|
altitude += _sitl->sonar_noise * rand_float();
|
||||||
|
|
||||||
// Altitude in in m, scaler in meters/volt
|
// Altitude in in m, scaler in meters/volt
|
||||||
voltage = altitude / _sitl->sonar_scale;
|
voltage = altitude / _sitl->sonar_scale;
|
||||||
voltage = constrain_float(voltage, 0, 5.0f);
|
// constrain to 0-5V
|
||||||
|
voltage = constrain_float(voltage, 0.0f, 5.0f);
|
||||||
|
|
||||||
if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) {
|
// Use glitch defines as the probablility between 0-1 that any given sonar sample will read as max distance
|
||||||
|
if (!is_zero(_sitl->sonar_glitch) && _sitl->sonar_glitch >= (rand_float() + 1.0f) / 2.0f) {
|
||||||
voltage = 5.0f;
|
voltage = 5.0f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
sonar_pin_value = 1023*(voltage / 5.0f);
|
sonar_pin_value = 1023 * (voltage / 5.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue