AP_Proximity: integrate oadb ekf-offset change

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -248,14 +248,14 @@ bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
 }
 
 // returns true if database is ready to be pushed to and all cached data is ready
-bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, float &current_heading)
+bool AP_Proximity_Backend::database_prepare_for_push(Vector2f &current_pos, float &current_heading)
 {
     AP_OADatabase *oaDb = AP::oadatabase();
     if (oaDb == nullptr || !oaDb->healthy()) {
         return false;
     }
 
-    if (!AP::ahrs().get_position(current_loc)) {
+    if (!AP::ahrs().get_relative_position_NE_origin(current_pos)) {
         return false;
     }
 
@@ -266,22 +266,22 @@ bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, floa
 // update Object Avoidance database with Earth-frame point
 void AP_Proximity_Backend::database_push(float angle, float distance)
 {
-    Location current_loc;
+    Vector2f current_pos;
     float current_heading;
-    if (database_prepare_for_push(current_loc, current_heading)) {
-        database_push(angle, distance, AP_HAL::millis(), current_loc, current_heading);
+    if (database_prepare_for_push(current_pos, current_heading)) {
+        database_push(angle, distance, AP_HAL::millis(), current_pos, current_heading);
     }
 }
 
 // update Object Avoidance database with Earth-frame point
-void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Location &current_loc, float current_heading)
+void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f &current_pos, float current_heading)
 {
     AP_OADatabase *oaDb = AP::oadatabase();
     if (oaDb == nullptr || !oaDb->healthy()) {
         return;
     }
 
-    Location temp_loc = current_loc;
-    temp_loc.offset_bearing(wrap_180(current_heading + angle), distance);
-    oaDb->queue_push(temp_loc, timestamp_ms, angle, distance);
+    Vector2f temp_pos = current_pos;
+    temp_pos.offset_bearing(wrap_180(current_heading + angle), distance);
+    oaDb->queue_push(temp_pos, timestamp_ms, distance);
 }

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -84,9 +84,9 @@ protected:
     bool ignore_reading(uint16_t angle_deg) const;
 
     // database helpers.  all angles are in degrees
-    bool database_prepare_for_push(Location &current_loc, float &current_heading);
+    bool database_prepare_for_push(Vector2f &current_pos, float &current_heading);
     void database_push(float angle, float distance);
-    void database_push(float angle, float distance, uint32_t timestamp_ms, const Location &current_loc, float current_heading);
+    void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f &current_pos, float current_heading);
 
     AP_Proximity &frontend;
     AP_Proximity::Proximity_State &state;   // reference to this instances state

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -328,9 +328,9 @@ void AP_Proximity_LightWareSF40C::process_message()
         }
 
         // prepare to push to object database
-        Location current_loc;
+        Vector2f current_pos;
         float current_heading;
-        const bool database_ready = database_prepare_for_push(current_loc, current_heading);
+        const bool database_ready = database_prepare_for_push(current_pos, current_heading);
 
         // process each point
         const float angle_inc_deg = (1.0f / point_total) * 360.0f;
@@ -386,7 +386,7 @@ void AP_Proximity_LightWareSF40C::process_message()
             // send combined distance to object database
             if ((i+1 >= point_count) || (combined_count >= PROXIMITY_SF40C_COMBINE_READINGS)) {
                 if ((combined_dist_m < INT16_MAX) && database_ready) {
-                    database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_loc, current_heading);
+                    database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_pos, current_heading);
                 }
                 combined_count = 0;
                 combined_dist_m = INT16_MAX;

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -104,9 +104,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
             increment *= -1;
         }
 
-        Location current_loc;
+        Vector2f current_pos;
         float current_heading;
-        const bool database_ready = database_prepare_for_push(current_loc, current_heading);
+        const bool database_ready = database_prepare_for_push(current_pos, current_heading);
 
         // initialise updated array and proximity sector angles (to closest object) and distances
         bool sector_updated[PROXIMITY_NUM_SECTORS];
@@ -144,7 +144,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
 
             // update Object Avoidance database with Earth-frame point
             if (database_ready) {
-                database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_heading);
+                database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, current_heading);
             }
         }