From ae7fb93e4c47b7b7eed73e0643f1a9a257853a9f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 5 Feb 2021 12:46:44 +1100
Subject: [PATCH] AP_HAL_SITL: add simulated RPLidarA1

---
 libraries/AP_HAL_SITL/SITL_State.cpp | 13 +++++++++++++
 libraries/AP_HAL_SITL/SITL_State.h   |  4 ++++
 libraries/SITL/SIM_PS_RPLidar.h      |  4 ++--
 libraries/SITL/SIM_PS_RPLidarA1.h    |  7 +++++++
 4 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 91a748be41..bf2ee37218 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -374,6 +374,14 @@ SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char *
         rplidara2 = new SITL::PS_RPLidarA2();
         return rplidara2;
 #endif
+#if HAL_SIM_PS_RPLIDARA1_ENABLED
+    } else if (streq(name, "rplidara1")) {
+        if (rplidara1 != nullptr) {
+            AP_HAL::panic("Only one rplidara1 at a time");
+        }
+        rplidara1 = new SITL::PS_RPLidarA1();
+        return rplidara1;
+#endif
 #if HAL_SIM_PS_TERARANGERTOWER_ENABLED
     } else if (streq(name, "terarangertower")) {
         if (terarangertower != nullptr) {
@@ -678,6 +686,11 @@ void SITL_State::_fdm_input_local(void)
     }
 #endif
 
+#if HAL_SIM_PS_RPLIDARA1_ENABLED
+    if (rplidara1 != nullptr) {
+        rplidara1->update(sitl_model->get_location());
+    }
+#endif
 #if HAL_SIM_PS_TERARANGERTOWER_ENABLED
     if (terarangertower != nullptr) {
         terarangertower->update(sitl_model->get_location());
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index d52397b7af..63984e7122 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -55,6 +55,7 @@
 // #include <SITL/SIM_Frsky_SPort.h>
 // #include <SITL/SIM_Frsky_SPortPassthrough.h>
 #include <SITL/SIM_PS_RPLidarA2.h>
+#include <SITL/SIM_PS_RPLidarA1.h>
 #include <SITL/SIM_PS_TeraRangerTower.h>
 #include <SITL/SIM_PS_LightWare_SF45B.h>
 
@@ -257,6 +258,9 @@ private:
     // simulated FETtec OneWire ESCs:
     SITL::FETtecOneWireESC *fetteconewireesc;
 
+    // simulated RPLidarA1:
+    SITL::PS_RPLidarA1 *rplidara1;
+
 #if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
     // simulated SF45B proximity sensor:
     SITL::PS_LightWare_SF45B *sf45b;
diff --git a/libraries/SITL/SIM_PS_RPLidar.h b/libraries/SITL/SIM_PS_RPLidar.h
index 815ed6c9c2..4758567605 100644
--- a/libraries/SITL/SIM_PS_RPLidar.h
+++ b/libraries/SITL/SIM_PS_RPLidar.h
@@ -51,7 +51,7 @@ rc 2 1450
 #define HAL_SIM_PS_RPLIDARA2_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED
 #endif
 
-#if HAL_SIM_PS_RPLIDARA2_ENABLED
+#if HAL_SIM_PS_RPLIDARA2_ENABLED || HAL_SIM_PS_RPLIDARA1_ENABLED
 
 #include <stdio.h>
 
@@ -144,4 +144,4 @@ private:
 
 };
 
-#endif  // HAL_SIM_PS_RPLIDARA2_ENABLED
+#endif  // HAL_SIM_PS_RPLIDARA2_ENABLED || HAL_SIM_PS_RPLIDARA1_ENABLED
diff --git a/libraries/SITL/SIM_PS_RPLidarA1.h b/libraries/SITL/SIM_PS_RPLidarA1.h
index 457206733f..9a88ca4451 100644
--- a/libraries/SITL/SIM_PS_RPLidarA1.h
+++ b/libraries/SITL/SIM_PS_RPLidarA1.h
@@ -45,6 +45,11 @@ rc 2 1450
 
 #include "SIM_PS_RPLidar.h"
 
+#ifndef HAL_SIM_PS_RPLIDARA1_ENABLED
+#define HAL_SIM_PS_RPLIDARA1_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED
+#endif
+
+#if HAL_SIM_PS_RPLIDARA1_ENABLED
 namespace SITL {
 
 class PS_RPLidarA1 : public PS_RPLidar {
@@ -54,3 +59,5 @@ public:
 };
 
 }
+
+#endif  // HAL_SIM_PS_RPLIDARA1_ENABLED