You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
1.3 KiB
57 lines
1.3 KiB
/* |
|
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_value = 1023 * (_sonar_pin_voltage() / 5.0f); |
|
} |
|
|
|
#endif
|
|
|