From b8dcdca909003a2e6460def61cd9f6c45e414b90 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 28 Jun 2019 22:57:09 -0700 Subject: [PATCH] AP_Proximity: add Object Avoidance Database, remove old Sector->Location converter --- libraries/AP_Proximity/AP_Proximity.cpp | 20 ---- libraries/AP_Proximity/AP_Proximity.h | 16 ---- .../AP_Proximity/AP_Proximity_Backend.cpp | 92 ++++++++----------- libraries/AP_Proximity/AP_Proximity_Backend.h | 20 ++-- .../AP_Proximity_LightWareSF40C.cpp | 2 +- libraries/AP_Proximity/AP_Proximity_MAV.cpp | 25 +++-- .../AP_Proximity/AP_Proximity_MorseSITL.cpp | 2 +- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 2 +- .../AP_Proximity/AP_Proximity_RangeFinder.cpp | 8 +- libraries/AP_Proximity/AP_Proximity_SITL.cpp | 2 +- .../AP_Proximity_TeraRangerTower.cpp | 2 +- .../AP_Proximity_TeraRangerTowerEvo.cpp | 2 +- 12 files changed, 71 insertions(+), 122 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index bba180b55b..edfc1a6c5e 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -379,26 +379,6 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const return get_boundary_points(primary_instance, num_points); } -// copy location points around vehicle into a buffer owned by the caller -// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) -// num_copied is updated with the number of locations copied into the buffer -// returns true on success, false on failure (should only happen if there is a semaphore conflict) -bool AP_Proximity::copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied) -{ - if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) { - num_copied = 0; - return false; - } - - // call backend copy_locations - return drivers[instance]->copy_locations(buff, buff_size, num_copied); -} - -bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied) -{ - return copy_locations(primary_instance, buff, buff_size, num_copied); -} - // get distance and angle to closest object (used for pre-arm check) // returns true on success, false if no valid readings bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 1d35418f38..5efafec0ea 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -20,13 +20,11 @@ #include #include #include -#include #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored #define PROXIMITY_MAX_DIRECTION 8 #define PROXIMITY_SENSOR_ID_START 10 -#define PROXIMITY_LOCATION_TIMEOUT_MS 3000 // locations (provided by copy_locations method) are valid for this many milliseconds class AP_Proximity_Backend; @@ -65,13 +63,6 @@ public: float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters }; - // structure holding locations of detected objects in earth frame - struct Proximity_Location { - float radius_m; // radius of object in meters - Location loc; - uint32_t last_update_ms; - }; - // detect and initialise any available proximity sensors void init(void); @@ -108,13 +99,6 @@ public: const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const; const Vector2f* get_boundary_points(uint16_t& num_points) const; - // copy location points around vehicle into a buffer owned by the caller - // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) - // num_copied is updated with the number of locations copied into the buffer - // returns true on success, false on failure (should only happen if there is a semaphore conflict) - bool copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied); - bool copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied); - // get distance and angle to closest object (used for pre-arm check) // returns true on success, false if no valid readings bool get_closest_object(float& angle_deg, float &distance) const; diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 3df9aa64b9..d9b5f63529 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "AP_Proximity.h" #include "AP_Proximity_Backend.h" @@ -174,19 +175,22 @@ void AP_Proximity_Backend::init_boundary() _sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f; _boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT; } - update_locations(); } // update boundary points used for object avoidance based on a single sector's distance changing // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle -void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector) +void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB) { // sanity check if (sector >= _num_sectors) { return; } + if (push_to_OA_DB) { + database_push(_angle[sector], _distance[sector]); + } + // find adjacent sector (clockwise) uint8_t next_sector = sector + 1; if (next_sector >= _num_sectors) { @@ -229,27 +233,6 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector) if (!_distance_valid[prev_sector_ccw]) { _boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance; } - update_locations(); -} - -// copy location points around vehicle into a buffer owned by the caller -// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) -// num_copied is updated with the number of locations copied into the buffer -// returns true on success, false on failure which should only happen if buff is nullptr -bool AP_Proximity_Backend::copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied) -{ - // sanity check buffer - if (buff == nullptr) { - num_copied = 0; - return false; - } - - WITH_SEMAPHORE(_rsem); - - // copy locations into caller's buffer - num_copied = MIN(_location_count, buff_size); - memcpy(buff, _locations, num_copied * sizeof(AP_Proximity::Proximity_Location)); - return true; } // set status and update valid count @@ -351,40 +334,41 @@ bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, in return found; } -void AP_Proximity_Backend::update_locations() +// 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_vehicle_bearing) { - WITH_SEMAPHORE(_rsem); + AP_OADatabase *oaDb = AP::oadatabase(); + if (oaDb == nullptr || !oaDb->healthy()) { + return false; + } - // get number of objects, return early if none - const uint8_t num_objects = get_object_count(); - if (num_objects == 0) { - _location_count = 0; - return; + if (!AP::ahrs().get_position(current_loc)) { + return false; } - // get vehicle location and heading in degrees - float veh_bearing; - Location my_loc; - { - WITH_SEMAPHORE(AP::ahrs().get_semaphore()); - if (!AP::ahrs().get_position(my_loc)) { - return; - } - veh_bearing = AP::ahrs().yaw_sensor * 0.01f; - } - - // convert offset from vehicle position to earth frame location - const uint32_t now_ms = AP_HAL::millis(); - _location_count = 0; - for (uint8_t i=0; ihealthy()) { + return; + } + + Location temp_loc = current_loc; + temp_loc.offset_bearing(wrap_180(current_vehicle_bearing + angle), distance); + oaDb->queue_push(temp_loc, timestamp_ms, distance, angle); +} diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 15ecd32a8f..67b3bcd218 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -17,6 +17,7 @@ #include #include #include "AP_Proximity.h" +#include #define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors #define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary. @@ -64,12 +65,6 @@ public: // get distances in 8 directions. used for sending distances to ground station bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const; - // copy location points around vehicle into a buffer owned by the caller - // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION) - // num_copied is updated with the number of locations copied into the buffer - // returns true on success, false on failure which should only happen if buff is nullptr - bool copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied); - protected: // set status and update valid_count @@ -85,15 +80,17 @@ protected: // update boundary points used for object avoidance based on a single sector's distance changing // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle - void update_boundary_for_sector(uint8_t sector); + void update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB); // get ignore area info uint8_t get_ignore_area_count() const; bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const; bool get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const; - // earth frame objects - void update_locations(); + // database helpers + bool database_prepare_for_push(Location ¤t_loc, float ¤t_vehicle_bearing); + void database_push(const float angle, const float distance); + void database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location ¤t_loc, const float current_vehicle_bearing); AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state @@ -111,9 +108,4 @@ protected: // fence boundary Vector2f _sector_edge_vector[PROXIMITY_SECTORS_MAX]; // vector for right-edge of each sector, used to speed up calculation of boundary Vector2f _boundary_point[PROXIMITY_SECTORS_MAX]; // bounding polygon around the vehicle calculated conservatively for object avoidance - - // earth frame locations (i.e. detected obstacles stored as lat/lon points) - uint16_t _location_count; // number of locations held in _locations buffer - AP_Proximity::Proximity_Location _locations[PROXIMITY_SECTORS_MAX]; // buffer of locations - HAL_Semaphore_Recursive _rsem; // semaphore for access to _locations and _location_count }; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 398953b526..82076d17f9 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -418,7 +418,7 @@ bool AP_Proximity_LightWareSF40C::process_reply() _last_distance_received_ms = AP_HAL::millis(); success = true; // update boundary used for avoidance - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } break; } diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index d38240e2cb..0a91feff5b 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -67,17 +67,17 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) { uint8_t sector = packet.orientation; _angle[sector] = sector * 45; - _distance[sector] = packet.current_distance / 100.0f; - _distance_min = packet.min_distance / 100.0f; - _distance_max = packet.max_distance / 100.0f; + _distance[sector] = packet.current_distance * 0.01f; + _distance_min = packet.min_distance * 0.01f; + _distance_max = packet.max_distance * 0.01f; _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max); _last_update_ms = AP_HAL::millis(); - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } // store upward distance if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) { - _distance_upward = packet.current_distance / 100.0f; + _distance_upward = packet.current_distance * 0.01f; _last_upward_update_ms = AP_HAL::millis(); } return; @@ -104,8 +104,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) const float total_distances = MIN(360.0f / increment, MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72 // set distance min and max - _distance_min = packet.min_distance / 100.0f; - _distance_max = packet.max_distance / 100.0f; + _distance_min = packet.min_distance * 0.01f; + _distance_max = packet.max_distance * 0.01f; _last_update_ms = AP_HAL::millis(); // get user configured yaw correction from front end @@ -115,6 +115,10 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) increment *= -1; } + Location current_loc; + float current_vehicle_bearing; + const bool database_ready = database_prepare_for_push(current_loc, current_vehicle_bearing); + // initialise updated array and proximity sector angles (to closest object) and distances bool sector_updated[_num_sectors]; float sector_width_half[_num_sectors]; @@ -140,13 +144,18 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) sector_updated[i] = true; } } + + // update Object Avoidance database with Earth-frame point + if (database_ready) { + database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_vehicle_bearing); + } } // update proximity sectors validity and boundary point for (uint8_t i = 0; i < _num_sectors; i++) { _distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max); if (sector_updated[i]) { - update_boundary_for_sector(i); + update_boundary_for_sector(i, false); } } } diff --git a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp index cbb94cad5e..dea8ac5ea4 100644 --- a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp @@ -69,7 +69,7 @@ void AP_Proximity_MorseSITL::update(void) _distance_valid[sector] = true; _distance[sector] = range; _angle[sector] = angle_deg; - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } } diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 82f598ddb3..a8ebaac088 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -416,7 +416,7 @@ void AP_Proximity_RPLidarA2::parse_response_data() _distance[_last_sector] = _distance_m_last; _distance_valid[_last_sector] = true; // update boundary used for avoidance - update_boundary_for_sector(_last_sector); + update_boundary_for_sector(_last_sector, true); // initialize the new sector _last_sector = sector; _distance_m_last = distance_m; diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index b1dc1a03a7..27bbb0dd7d 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -52,12 +52,12 @@ void AP_Proximity_RangeFinder::update(void) if (sensor->orientation() <= ROTATION_YAW_315) { uint8_t sector = (uint8_t)sensor->orientation(); _angle[sector] = sector * 45; - _distance[sector] = sensor->distance_cm() / 100.0f; - _distance_min = sensor->min_distance_cm() / 100.0f; - _distance_max = sensor->max_distance_cm() / 100.0f; + _distance[sector] = sensor->distance_cm() * 0.01f; + _distance_min = sensor->min_distance_cm() * 0.01f; + _distance_max = sensor->max_distance_cm() * 0.01f; _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max); _last_update_ms = now; - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } // check upward facing range finder if (sensor->orientation() == ROTATION_PITCH_90) { diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 7fc9c0476c..595d81c107 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -57,7 +57,7 @@ void AP_Proximity_SITL::update(void) set_status(AP_Proximity::Proximity_Good); _distance_valid[last_sector] = true; _angle[last_sector] = _sector_middle_deg[last_sector]; - update_boundary_for_sector(last_sector); + update_boundary_for_sector(last_sector, true); } else { _distance_valid[last_sector] = false; } diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index 3664228d2d..a8b31faf55 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -124,6 +124,6 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_ _distance_valid[sector] = distance_cm != 0xffff; _last_distance_received_ms = AP_HAL::millis(); // update boundary used for avoidance - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } } diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index feced2dd50..0da1d247a6 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -178,6 +178,6 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint _distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001; _last_distance_received_ms = AP_HAL::millis(); // update boundary used for avoidance - update_boundary_for_sector(sector); + update_boundary_for_sector(sector, true); } }