AP_Proximity: added a MorseSITL sensor

this interfaces with the laser scanner in morse

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
 }
 

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 {

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 <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_Proximity_MorseSITL.h"
+#include <stdio.h>
+
+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<points.length; i++) {
+        Vector3f &point = points.data[i];
+        float &range = ranges.data[i];
+        distance_maximum = MAX(distance_maximum, range);
+        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 = angle_rounded / degrees_per_sector;
+        if (!_distance_valid[sector] || range < _distance[sector]) {
+            _distance_valid[sector] = true;
+            _distance[sector] = range;
+            _angle[sector] = angle_deg;
+            update_boundary_for_sector(sector);
+        }
+    }
+
+#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_MorseSITL::distance_max() const
+{
+    // we don't have a data field from Morse for max range, so we use the max
+    // we've ever seen
+    return distance_maximum;
+}
+
+float AP_Proximity_MorseSITL::distance_min() const
+{
+    return 0.0f;
+}
+
+// get distance upwards in meters. returns true on success
+bool AP_Proximity_MorseSITL::get_upward_distance(float &distance) const
+{
+    // we don't have an upward facing laser
+    return false;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_Proximity/AP_Proximity_MorseSITL.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_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