mirror of https://github.com/ArduPilot/ardupilot
58 lines
1.3 KiB
C++
58 lines
1.3 KiB
C++
/*
|
|
SITL handling
|
|
|
|
This simulates a rangefinder
|
|
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
|
|
|
#include "AP_HAL_SITL.h"
|
|
#include "AP_HAL_SITL_Namespace.h"
|
|
#include "HAL_SITL_Class.h"
|
|
#include "SITL_State.h"
|
|
#include <SITL/SITL.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace HALSITL;
|
|
|
|
// returns a voltage between 0V to 5V which should appear as the
|
|
// voltage from the sensor
|
|
float SITL_State::_sonar_pin_voltage() const
|
|
{
|
|
// 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) {
|
|
// glitched
|
|
return 5.0f;
|
|
}
|
|
|
|
const float altitude = sitl_model->rangefinder_range();
|
|
#pragma GCC diagnostic push
|
|
#pragma GCC diagnostic ignored "-Wfloat-equal"
|
|
if (altitude == INFINITY) {
|
|
return 5.0f;
|
|
}
|
|
#pragma GCC diagnostic pop
|
|
|
|
// Altitude in in m, scaler in meters/volt
|
|
const float voltage = altitude / _sitl->sonar_scale;
|
|
|
|
// constrain to 0-5V
|
|
return constrain_float(voltage, 0.0f, 5.0f);
|
|
}
|
|
|
|
/*
|
|
setup the rangefinder with new input
|
|
*/
|
|
void SITL_State::_update_rangefinder()
|
|
{
|
|
sonar_pin_voltage = _sonar_pin_voltage();
|
|
}
|
|
|
|
#endif
|