diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index dd2cac8837..6a68d2c5fe 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -21,6 +21,7 @@
#include "AP_Proximity_RangeFinder.h"
#include "AP_Proximity_MAV.h"
#include "AP_Proximity_SITL.h"
+#include "AP_Proximity_MorseSITL.h"
extern const AP_HAL::HAL &hal;
@@ -31,7 +32,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// @Param: _TYPE
// @DisplayName: Proximity type
// @Description: What type of proximity sensor is connected
- // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo
+ // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,10:SITL,11:MorseSITL
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0),
@@ -324,6 +325,11 @@ void AP_Proximity::detect_instance(uint8_t instance)
drivers[instance] = new AP_Proximity_SITL(*this, state[instance]);
return;
}
+ if (type == Proximity_Type_MorseSITL) {
+ state[instance].instance = instance;
+ drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]);
+ return;
+ }
#endif
}
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index bfa330b5ef..84c6987a1e 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -49,6 +49,7 @@ public:
Proximity_Type_RPLidarA2 = 5,
Proximity_Type_TRTOWEREVO = 6,
Proximity_Type_SITL = 10,
+ Proximity_Type_MorseSITL = 11,
};
enum Proximity_Status {
diff --git a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp
new file mode 100644
index 0000000000..6d4404adf6
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp
@@ -0,0 +1,104 @@
+/*
+ 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 "AP_Proximity_MorseSITL.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#define PROXIMITY_MAX_RANGE 200.0f
+#define PROXIMITY_ACCURACY 0.1f
+
+/*
+ The constructor also initialises the proximity sensor.
+*/
+AP_Proximity_MorseSITL::AP_Proximity_MorseSITL(AP_Proximity &_frontend,
+ AP_Proximity::Proximity_State &_state):
+ AP_Proximity_Backend(_frontend, _state),
+ sitl(AP::sitl())
+{
+}
+
+// update the state of the sensor
+void AP_Proximity_MorseSITL::update(void)
+{
+ SITL::vector3f_array &points = sitl->state.scanner.points;
+ SITL::float_array &ranges = sitl->state.scanner.ranges;
+ if (points.length != ranges.length ||
+ points.length == 0) {
+ set_status(AP_Proximity::Proximity_NoData);
+ return;
+ }
+
+ set_status(AP_Proximity::Proximity_Good);
+
+ memset(_distance_valid, 0, sizeof(_distance_valid));
+ memset(_angle, 0, sizeof(_angle));
+ memset(_distance, 0, sizeof(_distance));
+
+ // only use 8 sectors to match RPLidar
+ const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX);
+ const uint16_t degrees_per_sector = 360 / nsectors;
+
+ for (uint16_t i=0; i
+
+class AP_Proximity_MorseSITL : public AP_Proximity_Backend
+{
+
+public:
+ // constructor
+ AP_Proximity_MorseSITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
+
+ // update state
+ void update(void) override;
+
+ // get maximum and minimum distances (in meters) of sensor
+ float distance_max() const override;
+ float distance_min() const override;
+
+ // get distance upwards in meters. returns true on success
+ bool get_upward_distance(float &distance) const override;
+
+private:
+ SITL::SITL *sitl;
+ float distance_maximum;
+};
+#endif // CONFIG_HAL_BOARD