fix cherry pick update issues

uci-overRID · Oct 23, 2024 · 14ece0e · 14ece0e
1 parent 1a074c8
commit 14ece0e
Show file tree

Hide file tree

Showing 3 changed files with 1 addition and 99 deletions.
diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp
@@ -259,65 +259,6 @@ void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
     return add_obstacle(obstacle_timestamp_ms, src, src_id, loc, vel);
 }
 
-// bool mac_eq(uint8_t a[6], uint8_t b[6]) {
-//     return a[0] == b[0] &&
-//            a[1] == b[1] &&
-//            a[2] == b[2] &&
-//            a[3] == b[3] &&
-//            a[4] == b[4] &&
-//            a[5] == b[5];
-// }
-
-void AP_Avoidance::add_obstacle(uint32_t obstacle_timestamp_ms,
-                      const MAV_COLLISION_SRC src,
-                      uint8_t src_id[6],
-                      const Location &loc,
-                      const Vector3f &vel_ned) 
-{
-    if (! check_startup()) {
-        return;
-    }
-    uint32_t oldest_timestamp = std::numeric_limits<uint32_t>::max();
-    uint8_t oldest_index = 255; // avoid compiler warning with initialisation
-    int16_t index = -1;
-    uint8_t i;
-    for (i=0; i<_obstacle_count; i++) {
-        if (mac_eq(src_id, _obstacles[i].mac) &&
-            _obstacles[i].src == src) {
-            // pre-existing obstacle found; we will update its information
-            index = i;
-            break;
-        }
-        if (_obstacles[i].timestamp_ms < oldest_timestamp) {
-            oldest_timestamp = _obstacles[i].timestamp_ms;
-            oldest_index = i;
-        }
-    }
-    WITH_SEMAPHORE(_rsem);
-
-    if (index == -1) {
-        // existing obstacle not found.  See if we can store it anyway:
-        if (i <_obstacles_allocated) {
-            // have room to store more vehicles...
-            index = _obstacle_count++;
-        } else if (oldest_timestamp < obstacle_timestamp_ms) {
-            // replace this very old entry with this new data
-            index = oldest_index;
-        } else {
-            // no room for this (old?!) data
-            return;
-        }
-
-        _obstacles[index].src = src;
-        memcpy(_obstacles[index].mac, src_id, 6);
-    }
-
-    _obstacles[index]._location = loc;
-    _obstacles[index]._velocity = vel_ned;
-    _obstacles[index].timestamp_ms = obstacle_timestamp_ms;
-
-}
-
 #if AP_ODIDSCANNER_ENABLED
 bool mac_eq(uint8_t a[6], uint8_t b[6]) {
     return a[0] == b[0] &&
@@ -422,27 +363,6 @@ void AP_Avoidance::get_adsb_samples()
     }
 }
 
-void AP_Avoidance::get_odid_samples()
-{
-    //TODO: Process samples here
-    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Avoidance: Get %d drones", AP::vehicle()->odidscanner.get_count());
-    for(int i = 0;i<AP::vehicle()->odidscanner.get_count();i++) {
-        auto v = AP::vehicle()->odidscanner.get_vehicle(i);
-        uint8_t mac[6];
-        memcpy(mac, v.info.id_or_mac, 6);
-        Vector3f vel;
-        float cog = v.loc.direction;
-        vel[0] = v.loc.speed_horizontal * cosf(radians(cog));
-        vel[1] = v.loc.speed_horizontal * sinf(radians(cog));
-        vel[2] = v.loc.speed_vertical;
-        add_obstacle(v.last_update_ms,
-                    MAV_COLLISION_SRC_ODID,
-                    mac,
-                    AP::vehicle()->odidscanner.get_vehicle_location(i),
-                    vel);
-    }
-}
-
 float closest_approach_xy(const Location &my_loc,
                           const Vector3f &my_vel,
                           const Location &obstacle_loc,

diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp b/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp
@@ -21,16 +21,6 @@
 
 #define VEHICLE_TIMEOUT_MS 30000
 
-
-bool mac_eq(uint8_t a[6], uint8_t b[6]) {
-    return a[0] == b[0] &&
-           a[1] == b[1] &&
-           a[2] == b[2] &&
-           a[3] == b[3] &&
-           a[4] == b[4] &&
-           a[5] == b[5];
-}
-
 // TODO: Random default for mav_port needs fix
 AP_ODIDScanner::AP_ODIDScanner() : _mav_port(MAVLINK_PORT) {
 

diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -49,9 +49,7 @@
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
 #include <AP_Camera/AP_RunCam.h>
 #include <AP_OpenDroneID/AP_OpenDroneID.h>
-
-#include "AP_ODIDScanner/AP_ODIDScanner.h"
-
+#include <AP_ODIDScanner/AP_ODIDScanner.h>
 #include <AP_Hott_Telem/AP_Hott_Telem.h>
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include <AP_GyroFFT/AP_GyroFFT.h>
@@ -85,9 +83,6 @@
 
 #include <AP_IBus_Telem/AP_IBus_Telem.h>
 
-
-#include <AP_ODIDScanner/AP_ODIDScanner.h>
-
 class AP_DDS_Client;
 
 class AP_Vehicle : public AP_HAL::HAL::Callbacks {
@@ -417,9 +412,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
 #if AP_OPENDRONEID_ENABLED
     AP_OpenDroneID opendroneid;
 #endif
-public:
-    AP_ODIDScanner odidscanner;
-
 public:
 #if AP_ODIDSCANNER_ENABLED
     AP_ODIDScanner odidscanner;