From 8f1c9759b8957b0a5466783716c2f628217d315d Mon Sep 17 00:00:00 2001
From: Rajat Singhal <rajatsinghal564@gmail.com>
Date: Thu, 11 Jul 2019 03:45:58 +0530
Subject: [PATCH] AP_Proximity: Add AirSimSITL lidar sensor

---
 libraries/AP_Proximity/AP_Proximity.cpp       |  8 +-
 libraries/AP_Proximity/AP_Proximity.h         |  1 +
 .../AP_Proximity/AP_Proximity_AirSimSITL.cpp  | 83 +++++++++++++++++++
 .../AP_Proximity/AP_Proximity_AirSimSITL.h    | 30 +++++++
 4 files changed, 121 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
 create mode 100644 libraries/AP_Proximity/AP_Proximity_AirSimSITL.h

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index edfc1a6c5e..5e107b4e30 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -22,6 +22,7 @@
 #include "AP_Proximity_MAV.h"
 #include "AP_Proximity_SITL.h"
 #include "AP_Proximity_MorseSITL.h"
+#include "AP_Proximity_AirSimSITL.h"
 #include <AP_AHRS/AP_AHRS.h>
 
 extern const AP_HAL::HAL &hal;
@@ -33,7 +34,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,10:SITL,11:MorseSITL
+    // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,10:SITL,11:MorseSITL,12:AirSimSITL
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO("_TYPE",   1, AP_Proximity, _type[0], 0),
@@ -331,6 +332,11 @@ void AP_Proximity::detect_instance(uint8_t instance)
         drivers[instance] = new AP_Proximity_MorseSITL(*this, state[instance]);
         return;
     }
+    if (type == Proximity_Type_AirSimSITL) {
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
+        return;
+    }
 #endif
 }
 
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 5efafec0ea..a3e0e0cdd8 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -49,6 +49,7 @@ public:
         Proximity_Type_TRTOWEREVO = 6,
         Proximity_Type_SITL    = 10,
         Proximity_Type_MorseSITL = 11,
+        Proximity_Type_AirSimSITL = 12,
     };
 
     enum Proximity_Status {
diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
new file mode 100644
index 0000000000..e162c7dd73
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
@@ -0,0 +1,83 @@
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_Proximity_AirSimSITL.h"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define PROXIMITY_MAX_RANGE 100.0f
+#define PROXIMITY_ACCURACY 0.1f
+
+/* 
+   The constructor also initialises the proximity sensor. 
+*/
+AP_Proximity_AirSimSITL::AP_Proximity_AirSimSITL(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_AirSimSITL::update(void)
+{
+    SITL::vector3f_array &points = sitl->state.scanner.points;
+    if (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<points.length; i++) {
+        Vector3f &point = points.data[i];
+        if (point.is_zero()) {
+            continue;
+        }
+        float angle_deg = wrap_360(degrees(atan2f(-point.y, point.x)));
+        uint16_t angle_rounded = uint16_t(angle_deg+0.5);
+        uint8_t sector = wrap_360(angle_rounded + 22.5f) / degrees_per_sector;
+        if (!_distance_valid[sector] || PROXIMITY_MAX_RANGE < _distance[sector]) {
+            _distance_valid[sector] = true;
+            const Vector2f v = Vector2f(point.x, point.y);
+            _distance[sector] = v.length();
+            _angle[sector] = angle_deg;
+            update_boundary_for_sector(sector, true);
+        }
+    }
+
+#if 0
+    printf("npoints=%u\n", points.length);
+    for (uint16_t i=0; i<nsectors; i++) {
+        printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]);
+    }
+#endif
+}
+
+// get maximum and minimum distances (in meters) of primary sensor
+float AP_Proximity_AirSimSITL::distance_max() const
+{
+    return PROXIMITY_MAX_RANGE;
+}
+
+float AP_Proximity_AirSimSITL::distance_min() const
+{
+    return 0.0f;
+}
+
+// get distance upwards in meters. returns true on success
+bool AP_Proximity_AirSimSITL::get_upward_distance(float &distance) const
+{
+    // we don't have an upward facing laser
+    return false;
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h
new file mode 100644
index 0000000000..e705a63d42
--- /dev/null
+++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include "AP_Proximity.h"
+#include "AP_Proximity_Backend.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <SITL/SITL.h>
+
+class AP_Proximity_AirSimSITL : public AP_Proximity_Backend
+{
+
+public:
+    // constructor
+    AP_Proximity_AirSimSITL(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