AP_OADatabase: replace Location with offset from origin

libraries/AC_Avoidance/AP_OADatabase.cpp

@@ -133,7 +133,7 @@ void AP_OADatabase::update()
 }
 
 // push a location into the database
-void AP_OADatabase::queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance)
+void AP_OADatabase::queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance)
 {
     if (!healthy()) {
         return;
@@ -144,7 +144,7 @@ void AP_OADatabase::queue_push(const Location &loc, uint32_t timestamp_ms, float
         return;
     }
 
-    const OA_DbItem item = {loc, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
+    const OA_DbItem item = {pos, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
     {
         WITH_SEMAPHORE(_queue.sem);
         _queue.items->push(item);
@@ -328,7 +328,8 @@ bool AP_OADatabase::is_close_to_item_in_database(const uint16_t index, const OA_
         return false;
     }
 
-    return (_database.items[index].loc.get_distance(item.loc) < item.radius);
+    const float distance_sq = (_database.items[index].pos - item.pos).length_squared();
+    return ((distance_sq < sq(item.radius)) || (distance_sq < sq(_database.items[index].radius)));
 }
 
 // send ADSB_VEHICLE mavlink messages
@@ -374,12 +375,15 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_
             continue;
         }
 
+        // convert object's position as an offset from EKF origin to Location
+        const Location item_loc(Vector3f(_database.items[idx].pos.x * 100.0f, _database.items[idx].pos.y * 100.0f, 0));
+
         mavlink_msg_adsb_vehicle_send(chan,
             idx,
-            _database.items[idx].loc.lat,
-            _database.items[idx].loc.lng,
+            item_loc.lat,
+            item_loc.lng,
             0,                          // altitude_type
-            _database.items[idx].loc.alt,
+            0,                          // altitude is always zero
             0,                          // heading
             0,                          // hor_velocity
             0,                          // ver_velocity

libraries/AC_Avoidance/AP_OADatabase.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#include <AP_Common/Location.h>
+#include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
 #if !HAL_MINIMIZE_FEATURES
@@ -26,7 +26,7 @@ public:
     };
 
     struct OA_DbItem {
-        Location loc;           // location of object. TODO: turn this into Vector2Int to save memory
+        Vector2f pos;           // position of the object as an offset in meters from the EKF origin
         uint32_t timestamp_ms;  // system time that object was last updated
         float radius;           // objects radius in meters
         uint8_t send_to_gcs;    // bitmask of mavlink comports to which details of this object should be sent
@@ -36,8 +36,8 @@ public:
     void init();
     void update();
 
-    // push a location into the database
-    void queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance);
+    // push an object into the database.  Pos is the offset in meters from the EKF origin, angle is in degrees, distance in meters
+    void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance);
 
     // returns true if database is healthy
     bool healthy() const { return (_queue.items != nullptr) && (_database.items != nullptr); }
@@ -116,7 +116,7 @@ class AP_OADatabase {
 public:
     static AP_OADatabase *get_singleton() { return nullptr; }
     void init() {};
-    void queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance) {};
+    void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance) {};
     bool healthy() const { return false; }
     void send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms) {};
 };