From e18c195df464e8f99cda725d269d1f3c0ae91673 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 18 Jul 2020 00:36:47 +0100
Subject: [PATCH] AP_RangeFinder: add SITL backend

---
 libraries/AP_RangeFinder/AP_RangeFinder.cpp   |  7 +++
 libraries/AP_RangeFinder/AP_RangeFinder.h     |  1 +
 .../AP_RangeFinder/AP_RangeFinder_SITL.cpp    | 52 +++++++++++++++++++
 .../AP_RangeFinder/AP_RangeFinder_SITL.h      | 44 ++++++++++++++++
 4 files changed, 104 insertions(+)
 create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp
 create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_SITL.h

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
index b0ca9a4f5c..2fb031840e 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
@@ -45,6 +45,7 @@
 #include "AP_RangeFinder_UAVCAN.h"
 #include "AP_RangeFinder_Lanbao.h"
 #include "AP_RangeFinder_LeddarVu8.h"
+#include "AP_RangeFinder_SITL.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -534,6 +535,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         }
         break;
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case Type::SITL:
+        drivers[instance] = new AP_RangeFinder_SITL(state[instance], params[instance], instance);
+        break;
+#endif
+
     default:
         break;
     }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index 756a11eb1f..05fefcb22d 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -81,6 +81,7 @@ public:
         LeddarVu8_Serial = 29,
         HC_SR04 = 30,
         GYUS42v2 = 31,
+        SITL = 100,
     };
 
     enum class Function {
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp
new file mode 100644
index 0000000000..f8c2d5b64f
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.cpp
@@ -0,0 +1,52 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "AP_RangeFinder_SITL.h"
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  constructor - registers instance at top RangeFinder driver
+ */
+AP_RangeFinder_SITL::AP_RangeFinder_SITL(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, uint8_t instance) :
+    AP_RangeFinder_Backend(_state, _params),
+    sitl(AP::sitl()),
+    _instance(instance)
+{}
+
+/*
+  update distance_cm
+ */
+void AP_RangeFinder_SITL::update(void)
+{
+    const float dist = sitl->get_rangefinder(_instance);
+
+    // negative distance means nothing is connected
+    if (is_negative(dist)) {
+        state.status = RangeFinder::Status::NoData;
+        return;
+    }
+
+    state.distance_cm = dist * 100.0f;
+    state.last_reading_ms = AP_HAL::millis();
+
+    // update range_valid state based on distance measured
+    update_status();
+}
+
+#endif
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h
new file mode 100644
index 0000000000..41e6f91382
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_SITL.h
@@ -0,0 +1,44 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "AP_RangeFinder_Backend.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include <SITL/SITL.h>
+
+class AP_RangeFinder_SITL : public AP_RangeFinder_Backend {
+public:
+    // constructor. This incorporates initialisation as well.
+    AP_RangeFinder_SITL(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, uint8_t instance);
+
+    // update the state structure
+    void update() override;
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+
+private:
+    SITL::SITL *sitl;
+
+    uint8_t _instance;
+
+};
+
+#endif