From dac84f4a9d7ccdbf3009fe84373a0600aedff9b4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 4 Dec 2019 16:17:23 +0900 Subject: [PATCH] AP_Proximity: integrate oadb ekf-offset change --- .../AP_Proximity/AP_Proximity_Backend.cpp | 18 +++++++++--------- libraries/AP_Proximity/AP_Proximity_Backend.h | 4 ++-- .../AP_Proximity_LightWareSF40C.cpp | 6 +++--- libraries/AP_Proximity/AP_Proximity_MAV.cpp | 6 +++--- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 132fafe976..d598e7e333 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/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 ¤t_loc, float ¤t_heading) +bool AP_Proximity_Backend::database_prepare_for_push(Vector2f ¤t_pos, float ¤t_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 ¤t_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 ¤t_loc, float current_heading) +void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f ¤t_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); } diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 47ecc18c0f..949b7086ee 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/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 ¤t_loc, float ¤t_heading); + bool database_prepare_for_push(Vector2f ¤t_pos, float ¤t_heading); void database_push(float angle, float distance); - void database_push(float angle, float distance, uint32_t timestamp_ms, const Location ¤t_loc, float current_heading); + void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f ¤t_pos, float current_heading); AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index ec8b3379f1..0b10055f6a 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/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; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index f5765655c0..94f159d5eb 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -103,9 +103,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]; @@ -143,7 +143,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); } }