diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index 572bcec3b9..f5a11db804 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -15,6 +15,7 @@
#include "AP_Proximity.h"
#include "AP_Proximity_LightWareSF40C.h"
+#include "AP_Proximity_SITL.h"
extern const AP_HAL::HAL &hal;
@@ -167,6 +168,13 @@ void AP_Proximity::detect_instance(uint8_t instance)
return;
}
}
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ if (type == Proximity_Type_SITL) {
+ state[instance].instance = instance;
+ drivers[instance] = new AP_Proximity_SITL(*this, state[instance]);
+ return;
+ }
+#endif
}
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index c31e2677c4..7984051226 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -37,6 +37,7 @@ public:
enum Proximity_Type {
Proximity_Type_None = 0,
Proximity_Type_SF40C = 1,
+ Proximity_Type_SITL = 10,
};
enum Proximity_Status {
diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp
new file mode 100644
index 0000000000..6c6d9f059d
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp
@@ -0,0 +1,105 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include
+#include "AP_Proximity_SITL.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#define PROXIMITY_MAX_RANGE 200
+#define PROXIMITY_ACCURACY 0.1
+
+/*
+ The constructor also initialises the proximity sensor.
+*/
+AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
+ AP_Proximity::Proximity_State &_state):
+ AP_Proximity_Backend(_frontend, _state)
+{
+ sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
+ ap_var_type ptype;
+ fence_count = (AP_Int8 *)AP_Param::find("FENCE_TOTAL", &ptype);
+ if (fence_count == nullptr || ptype != AP_PARAM_INT8) {
+ AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL");
+ }
+}
+
+// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
+bool AP_Proximity_SITL::get_horizontal_distance(float angle_deg, float &distance) const
+{
+ if (!fence_loader.boundary_valid(fence_count->get(), fence, true)) {
+ return false;
+ }
+
+ // convert to earth frame
+ angle_deg = wrap_360(sitl->state.yawDeg + angle_deg);
+
+ /*
+ simple bisection search to find distance. Not really efficient,
+ but we can afford the CPU in SITL
+ */
+ float min_dist = 0, max_dist = PROXIMITY_MAX_RANGE;
+ while (max_dist - min_dist > PROXIMITY_ACCURACY) {
+ float test_dist = (max_dist+min_dist)*0.5f;
+ Location loc = current_loc;
+ location_update(loc, angle_deg, test_dist);
+ Vector2l vecloc(loc.lat, loc.lng);
+ if (fence_loader.boundary_breached(vecloc, fence_count->get(), fence, true)) {
+ max_dist = test_dist;
+ } else {
+ min_dist = test_dist;
+ }
+ }
+ distance = min_dist;
+ return true;
+}
+
+// update the state of the sensor
+void AP_Proximity_SITL::update(void)
+{
+ load_fence();
+ current_loc.lat = sitl->state.latitude * 1.0e7;
+ current_loc.lng = sitl->state.longitude * 1.0e7;
+ current_loc.alt = sitl->state.altitude * 1.0e2;
+ if (fence && fence_loader.boundary_valid(fence_count->get(), fence, true)) {
+ set_status(AP_Proximity::Proximity_Good);
+ } else {
+ set_status(AP_Proximity::Proximity_NoData);
+ }
+}
+
+void AP_Proximity_SITL::load_fence(void)
+{
+ uint32_t now = AP_HAL::millis();
+ if (now - last_load_ms < 1000) {
+ return;
+ }
+ last_load_ms = now;
+
+ if (fence == nullptr) {
+ fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l));
+ }
+ if (fence == nullptr) {
+ return;
+ }
+ for (uint8_t i=0; iget(); i++) {
+ fence_loader.load_point_from_eeprom(i, fence[i]);
+ }
+}
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h
new file mode 100644
index 0000000000..c21c42aedf
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_SITL.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include
+#include
+
+class AP_Proximity_SITL : public AP_Proximity_Backend
+{
+
+public:
+ // constructor
+ AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+ // get distance in meters in a particular direction in degrees (0 is forward, clockwise)
+ // returns true on successful read and places distance in distance
+ bool get_horizontal_distance(float angle_deg, float &distance) const override;
+
+ // update state
+ void update(void) override;
+
+private:
+ SITL::SITL *sitl;
+ Vector2l *fence;
+ AP_Int8 *fence_count;
+ uint32_t last_load_ms;
+ AC_PolyFence_loader fence_loader;
+ Location current_loc;
+
+ void load_fence(void);
+};
+#endif // CONFIG_HAL_BOARD