AP_Proximity: Add backend for scripted Lua Driver

libraries/AP_Proximity/AP_Proximity.cpp

@@ -28,6 +28,7 @@
 #include "AP_Proximity_AirSimSITL.h"
 #include "AP_Proximity_Cygbot_D1.h"
 #include "AP_Proximity_DroneCAN.h"
+#include "AP_Proximity_Scripting.h"
 
 #include <AP_Logger/AP_Logger.h>
 
@@ -198,6 +199,12 @@ void AP_Proximity::init()
             num_instances = instance+1;
         break;
 #endif
+#if AP_PROXIMITY_SCRIPTING_ENABLED
+        case Type::Scripting:
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_Scripting(*this, state[instance], params[instance]);
+        break;
+#endif
 #if AP_PROXIMITY_SITL_ENABLED
         case Type::SITL:
             state[instance].instance = instance;
@@ -272,6 +279,15 @@ AP_Proximity::Status AP_Proximity::get_status() const
     return Status::Good;
 }
 
+// return proximity backend for Lua scripting
+AP_Proximity_Backend *AP_Proximity::get_backend(uint8_t id) const
+{
+    if (!valid_instance(id)) {
+        return nullptr;
+    }
+    return drivers[id];
+}
+
 // prearm checks
 bool AP_Proximity::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
 {

libraries/AP_Proximity/AP_Proximity.h

@@ -80,6 +80,9 @@ public:
 #endif
 #if AP_PROXIMITY_DRONECAN_ENABLED
         DroneCAN = 14,
+#endif
+#if AP_PROXIMITY_SCRIPTING_ENABLED
+        Scripting = 15,
 #endif
     };
 
@@ -180,6 +183,9 @@ public:
 
     static AP_Proximity *get_singleton(void) { return _singleton; };
 
+    // return backend object for Lua scripting
+    AP_Proximity_Backend *get_backend(uint8_t id) const;
+
     // 3D boundary
     AP_Proximity_Boundary_3D boundary;
 

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -43,6 +43,18 @@ public:
     // handle mavlink messages
     virtual void handle_msg(const mavlink_message_t &msg) {}
 
+#if AP_SCRIPTING_ENABLED
+    // handle scripting obstacle messages
+    virtual bool set_distance_min_max(float min, float max) { return false; }
+    // this is in body frame
+    virtual bool handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary) { return false; }
+    virtual bool handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary) { return false; }
+    virtual bool update_virtual_boundary() { return false; }
+#endif
+
+    // return the type of sensor
+    AP_Proximity::Type type() const { return (AP_Proximity::Type)params.type.get(); }
+
 protected:
 
     // set status and update valid_count

libraries/AP_Proximity/AP_Proximity_Params.cpp

@@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Proximity type
     // @Description: What type of proximity sensor is connected
-    // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN
+    // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE",   1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE),

libraries/AP_Proximity/AP_Proximity_Scripting.cpp

@@ -0,0 +1,127 @@
+/*
+   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_Proximity_Scripting.h"
+
+#if HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <ctype.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define PROXIMITY_SCRIPTING_TIMEOUT_MS 1500 // distance messages must arrive within this many milliseconds
+
+// update the state of the sensor
+void AP_Proximity_Scripting::update(void)
+{
+    // check for timeout and set health status
+    if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_SCRIPTING_TIMEOUT_MS)) &&
+        (_last_upward_update_ms == 0 || (AP_HAL::millis() - _last_upward_update_ms > PROXIMITY_SCRIPTING_TIMEOUT_MS))) {
+        set_status(AP_Proximity::Status::NoData);
+    } else {
+        set_status(AP_Proximity::Status::Good);
+    }
+}
+
+// Set max and min range of the sensor. Only needs to be set once
+bool AP_Proximity_Scripting::set_distance_min_max(float min, float max)
+{
+    if (min >= max) {
+        return false;
+    }
+    _distance_min = min;
+    _distance_max = max;
+    return true;
+}
+
+// get distance upwards in meters. returns true on success
+bool AP_Proximity_Scripting::get_upward_distance(float &distance) const
+{
+    if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_SCRIPTING_TIMEOUT_MS)) {
+        distance = _distance_upward;
+        return true;
+    }
+    return false;
+}
+
+// handle script distance messages
+bool AP_Proximity_Scripting::handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary)
+{
+    _last_update_ms = AP_HAL::millis();
+
+    Vector3f current_pos;
+    Matrix3f body_to_ned;
+    const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
+
+    if (dist_m < distance_min() || dist_m > distance_max() || is_zero(dist_m)) {
+        // message isn't healthy
+        return false;
+    }
+
+    // store upward distance
+    if (is_equal(pitch_deg, 90.f)) {
+        _distance_upward = dist_m;
+        _last_upward_update_ms = _last_update_ms;
+        return true;
+    }
+
+    yaw_deg = correct_angle_for_orientation(yaw_deg);
+
+    if (ignore_reading(pitch_deg, yaw_deg, dist_m, false)) {
+        // obstacle is probably near ground or out of range
+        return false;
+    }
+
+    // allot to correct layer and sector based on calculated pitch and yaw
+    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(pitch_deg, yaw_deg);
+
+    // add to temp boundary
+    temp_boundary.add_distance(face, pitch_deg, yaw_deg, dist_m);
+
+    if (push_to_boundary) {
+        temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
+        temp_boundary.reset();
+    }
+
+    if (database_ready) {
+        database_push(yaw_deg, pitch_deg, dist_m, _last_update_ms, current_pos, body_to_ned);
+    }
+
+    return true;
+}
+
+// handle script vector messages
+bool AP_Proximity_Scripting::handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary)
+{
+    // convert to FRU
+    const Vector3f obstacle(vec_to_obstacle.x, vec_to_obstacle.y, vec_to_obstacle.z * -1.0f);
+
+    // extract yaw and pitch from Obstacle Vector
+    const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
+    const float pitch = wrap_180(degrees(M_PI_2 - atan2f(obstacle.xy().length(), obstacle.z)));
+
+    // now simply handle as a distance msg
+    return handle_script_distance_msg(obstacle.length(), yaw, pitch, push_to_boundary);
+}
+
+// update the temporary (buffer) boundary
+bool AP_Proximity_Scripting::update_virtual_boundary()
+{
+    temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
+    temp_boundary.reset();
+    return true;
+}
+
+#endif // HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED

libraries/AP_Proximity/AP_Proximity_Scripting.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "AP_Proximity_Backend.h"
+
+#if HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED
+
+class AP_Proximity_Scripting : public AP_Proximity_Backend
+{
+
+public:
+    // constructor
+    using AP_Proximity_Backend::AP_Proximity_Backend;
+
+    // update state
+    void update(void) override;
+
+    // get maximum and minimum distances (in meters) of sensor
+    float distance_max() const override { return _distance_max; }
+    float distance_min() const override { return _distance_min; }
+
+    // Set max and min range of the sensor. only needs to be set once
+    bool set_distance_min_max(float min, float max) override;
+
+    // get distance upwards in meters. returns true on success
+    bool get_upward_distance(float &distance) const override;
+
+    // handle script messages
+    bool handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary) override;
+    bool handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary) override;
+
+    // update the temporary (buffer) boundary
+    bool update_virtual_boundary() override;
+
+private:
+
+    // temp boundary to store and sort distances
+    AP_Proximity_Temp_Boundary temp_boundary;
+
+    // horizontal distance support
+    uint32_t _last_update_ms;   // system time of last script message received
+
+    // upward distance support
+    uint32_t _last_upward_update_ms;    // system time of last update of upward distance
+    float _distance_upward;             // upward distance in meters
+
+    // min and max distance of sensor
+    float _distance_min;
+    float _distance_max;
+};
+
+#endif // HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED

libraries/AP_Proximity/AP_Proximity_config.h

@@ -32,6 +32,10 @@
 #define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_PROXIMITY_SCRIPTING_ENABLED
+#define AP_PROXIMITY_SCRIPTING_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED
+#endif
+
 #ifndef AP_PROXIMITY_MAV_ENABLED
 #define AP_PROXIMITY_MAV_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED
 #endif