From 43cd419dcfeee370f27dd8c7beb4e871d35f3ae2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 4 Dec 2019 11:46:03 +0900 Subject: [PATCH] AP_Proximity: always use 8 sectors and fix ignore areas --- .../AP_Proximity/AP_Proximity_AirSimSITL.cpp | 8 +- .../AP_Proximity/AP_Proximity_Backend.cpp | 126 ++++-------------- libraries/AP_Proximity/AP_Proximity_Backend.h | 26 ++-- .../AP_Proximity_LightWareSF40C.cpp | 70 +--------- .../AP_Proximity_LightWareSF40C.h | 2 - .../AP_Proximity_LightWareSF40C_v09.cpp | 75 +---------- .../AP_Proximity_LightWareSF40C_v09.h | 2 - libraries/AP_Proximity/AP_Proximity_MAV.cpp | 12 +- .../AP_Proximity/AP_Proximity_MorseSITL.cpp | 8 +- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 72 +--------- .../AP_Proximity/AP_Proximity_RPLidarA2.h | 2 - libraries/AP_Proximity/AP_Proximity_SITL.cpp | 2 +- .../AP_Proximity_TeraRangerTower.cpp | 16 +-- .../AP_Proximity_TeraRangerTowerEvo.cpp | 18 ++- 14 files changed, 78 insertions(+), 361 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp index 44a1efde9d..f04ea9f41e 100644 --- a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp @@ -38,10 +38,6 @@ void AP_Proximity_AirSimSITL::update(void) memset(_angle, 0, sizeof(_angle)); memset(_distance, 0, sizeof(_distance)); - // only use 8 sectors to match RPLidar - const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX); - const uint16_t degrees_per_sector = 360 / nsectors; - for (uint16_t i=0; i(_angle[i] * (PROXIMITY_MAX_DIRECTION / 360.0f)); @@ -149,7 +147,7 @@ const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) // check at least one sector has valid data, if not, exit bool some_valid = false; - for (uint8_t i=0; i<_num_sectors; i++) { + for (uint8_t i=0; i= _num_sectors) { + if (sector >= PROXIMITY_NUM_SECTORS) { return; } @@ -193,7 +191,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons // find adjacent sector (clockwise) uint8_t next_sector = sector + 1; - if (next_sector >= _num_sectors) { + if (next_sector >= PROXIMITY_NUM_SECTORS) { next_sector = 0; } @@ -217,7 +215,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons } // repeat for edge between sector and previous sector - uint8_t prev_sector = (sector == 0) ? _num_sectors-1 : sector-1; + uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1; shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; if (_distance_valid[prev_sector] && _distance_valid[sector]) { shortest_distance = MIN(_distance[prev_sector], _distance[sector]); @@ -229,7 +227,7 @@ void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, cons _boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance; // if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary - uint8_t prev_sector_ccw = (prev_sector == 0) ? _num_sectors-1 : prev_sector-1; + uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1; if (!_distance_valid[prev_sector_ccw]) { _boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance; } @@ -241,97 +239,23 @@ void AP_Proximity_Backend::set_status(AP_Proximity::Status status) state.status = status; } -bool AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees, uint8_t §or) const +uint8_t AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees) const { - // sanity check angle - if (angle_degrees > 360.0f || angle_degrees < -180.0f) { - return false; - } - - // convert to 0 ~ 360 - if (angle_degrees < 0.0f) { - angle_degrees += 360.0f; - } - - bool closest_found = false; - uint8_t closest_sector; - float closest_angle; - - // search for which sector angle_degrees falls into - for (uint8_t i = 0; i < _num_sectors; i++) { - float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - angle_degrees)); - - // record if closest - if (!closest_found || angle_diff < closest_angle) { - closest_found = true; - closest_sector = i; - closest_angle = angle_diff; - } - - if (fabsf(angle_diff) <= _sector_width_deg[i] / 2.0f) { - sector = i; - return true; - } - } - - // angle_degrees might have been within a gap between sectors - if (closest_found) { - sector = closest_sector; - return true; - } - - return false; + return wrap_360(angle_degrees + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f; } -// get ignore area info -uint8_t AP_Proximity_Backend::get_ignore_area_count() const +// check if a reading should be ignored because it falls into an ignore area +bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const { - // count number of ignore sectors - uint8_t count = 0; + // check angle vs each ignore area for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) { if (frontend._ignore_width_deg[i] != 0) { - count++; - } - } - return count; -} - -// get next ignore angle -bool AP_Proximity_Backend::get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const -{ - if (index >= PROXIMITY_MAX_IGNORE) { - return false; - } - angle_deg = frontend._ignore_angle_deg[index]; - width_deg = frontend._ignore_width_deg[index]; - return true; -} - -// retrieve start or end angle of next ignore area (i.e. closest ignore area higher than the start_angle) -// start_or_end = 0 to get start, 1 to retrieve end -bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const -{ - bool found = false; - int16_t smallest_angle_diff = 0; - int16_t smallest_angle_start = 0; - - for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) { - if (frontend._ignore_width_deg[i] != 0) { - int16_t offset = start_or_end == 0 ? -frontend._ignore_width_deg[i] : +frontend._ignore_width_deg[i]; - int16_t ignore_start_angle = wrap_360(frontend._ignore_angle_deg[i] + offset/2.0f); - int16_t ang_diff = wrap_360(ignore_start_angle - start_angle); - if (!found || ang_diff < smallest_angle_diff) { - smallest_angle_diff = ang_diff; - smallest_angle_start = ignore_start_angle; - found = true; + if (abs(angle_deg - frontend._ignore_width_deg[i]) <= (frontend._ignore_width_deg[i]/2)) { + return true; } } } - - if (found) { - ignore_start = smallest_angle_start; - } - return found; + return false; } // returns true if database is ready to be pushed to and all cached data is ready diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index c9784cd82e..f0a4024976 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -19,7 +19,8 @@ #include "AP_Proximity.h" #include -#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors +#define PROXIMITY_NUM_SECTORS 8 // number of sectors +#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // width of sectors in degrees #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. #define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out @@ -71,7 +72,7 @@ protected: void set_status(AP_Proximity::Status status); // find which sector a given angle falls into - bool convert_angle_to_sector(float angle_degrees, uint8_t §or) const; + uint8_t convert_angle_to_sector(float angle_degrees) const; // initialise the boundary and sector_edge_vector array used for object avoidance // should be called if the sector_middle_deg or _setor_width_deg arrays are changed @@ -82,10 +83,9 @@ protected: // 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(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; + // check if a reading should be ignored because it falls into an ignore area + // angles should be in degrees and in the range of 0 to 360 + 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); @@ -96,16 +96,14 @@ protected: AP_Proximity::Proximity_State &state; // reference to this instances state // sectors - uint8_t _num_sectors = PROXIMITY_MAX_DIRECTION; - uint16_t _sector_middle_deg[PROXIMITY_SECTORS_MAX] = {0, 45, 90, 135, 180, 225, 270, 315, 0, 0, 0, 0}; // middle angle of each sector - uint8_t _sector_width_deg[PROXIMITY_SECTORS_MAX] = {45, 45, 45, 45, 45, 45, 45, 45, 0, 0, 0, 0}; // width (in degrees) of each sector + const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] = {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector // sensor data - float _angle[PROXIMITY_SECTORS_MAX]; // angle to closest object within each sector - float _distance[PROXIMITY_SECTORS_MAX]; // distance to closest object within each sector - bool _distance_valid[PROXIMITY_SECTORS_MAX]; // true if a valid distance received for each sector + float _angle[PROXIMITY_NUM_SECTORS]; // angle to closest object within each sector + float _distance[PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector + bool _distance_valid[PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector // 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 + Vector2f _sector_edge_vector[PROXIMITY_NUM_SECTORS]; // vector for right-edge of each sector, used to speed up calculation of boundary + Vector2f _boundary_point[PROXIMITY_NUM_SECTORS]; // bounding polygon around the vehicle calculated conservatively for object avoidance }; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 47acd4f6f3..85b6ffb9a9 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -73,10 +73,8 @@ void AP_Proximity_LightWareSF40C::update(void) // initialise sensor void AP_Proximity_LightWareSF40C::initialise() { - // initialise sectors - if (!_sector_initialised) { - init_sectors(); - } + // initialise boundary + init_boundary(); // exit immediately if we've sent initialisation requests in the last second uint32_t now_ms = AP_HAL::millis(); @@ -137,66 +135,6 @@ void AP_Proximity_LightWareSF40C::restart_sensor() _sensor_state.output_rate = 0; } -// initialise sector angles using user defined ignore areas -void AP_Proximity_LightWareSF40C::init_sectors() -{ - // use defaults if no ignore areas defined - uint8_t ignore_area_count = get_ignore_area_count(); - if (ignore_area_count == 0) { - _sector_initialised = true; - return; - } - - uint8_t sector = 0; - - for (uint8_t i=0; i 0) && (sector < PROXIMITY_SECTORS_MAX)) { - uint16_t sector_size; - if (degrees_to_fill >= 90) { - // set sector to maximum of 45 degrees - sector_size = 45; - } else if (degrees_to_fill > 45) { - // use half the remaining area to optimise size of this sector and the next - sector_size = degrees_to_fill / 2.0f; - } else { - // 45 degrees or less are left so put it all into the next sector - sector_size = degrees_to_fill; - } - // record the sector middle and width - _sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f); - _sector_width_deg[sector] = sector_size; - - // move onto next sector - start_angle += sector_size; - sector++; - degrees_to_fill -= sector_size; - } - } - } - - // set num sectors - _num_sectors = sector; - - // re-initialise boundary because sector locations have changed - init_boundary(); - - // record success - _sector_initialised = true; -} - // send message to sensor void AP_Proximity_LightWareSF40C::send_message(MessageID msgid, bool write, const uint8_t *payload, uint16_t payload_len) { @@ -404,8 +342,8 @@ void AP_Proximity_LightWareSF40C::process_message() const uint16_t idx = 14 + (i * 2); const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]); const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction); - uint8_t sector; - if (convert_angle_to_sector(angle_deg, sector)) { + if (!ignore_reading(angle_deg)) { + const uint8_t sector = convert_angle_to_sector(angle_deg); if (sector != _last_sector) { // update boundary used for avoidance if (_last_sector != UINT8_MAX) { diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index dc58fc1f7f..9a68b6cec6 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h @@ -28,7 +28,6 @@ private: // initialise sensor void initialise(); - void init_sectors(); // restart sensor and re-init our state void restart_sensor(); @@ -106,7 +105,6 @@ private: // internal variables AP_HAL::UARTDriver *_uart; // uart for communicating with sensor - bool _sector_initialised; // true if sectors have been initialised uint32_t _last_request_ms; // system time of last request uint32_t _last_reply_ms; // system time of last valid reply uint32_t _last_restart_ms; // system time we restarted the sensor diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp index bca01b37b4..3468d9e3ff 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp @@ -101,72 +101,11 @@ bool AP_Proximity_LightWareSF40C_v09::initialise() } return false; } + // initialise sectors - if (!_sector_initialised) { - init_sectors(); - return false; - } - return true; -} - -// initialise sector angles using user defined ignore areas -void AP_Proximity_LightWareSF40C_v09::init_sectors() -{ - // use defaults if no ignore areas defined - uint8_t ignore_area_count = get_ignore_area_count(); - if (ignore_area_count == 0) { - _sector_initialised = true; - return; - } - - uint8_t sector = 0; - - for (uint8_t i=0; i 0) && (sector < PROXIMITY_SECTORS_MAX)) { - uint16_t sector_size; - if (degrees_to_fill >= 90) { - // set sector to maximum of 45 degrees - sector_size = 45; - } else if (degrees_to_fill > 45) { - // use half the remaining area to optimise size of this sector and the next - sector_size = degrees_to_fill / 2.0f; - } else { - // 45 degrees or less are left so put it all into the next sector - sector_size = degrees_to_fill; - } - // record the sector middle and width - _sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f); - _sector_width_deg[sector] = sector_size; - - // move onto next sector - start_angle += sector_size; - sector++; - degrees_to_fill -= sector_size; - } - } - } - - // set num sectors - _num_sectors = sector; - - // re-initialise boundary because sector locations have changed init_boundary(); - // record success - _sector_initialised = true; + return true; } // set speed of rotating motor @@ -281,15 +220,15 @@ bool AP_Proximity_LightWareSF40C_v09::send_request_for_distance() // increment sector _last_sector++; - if (_last_sector >= _num_sectors) { + if (_last_sector >= PROXIMITY_NUM_SECTORS) { _last_sector = 0; } // prepare request char request_str[16]; snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n", - MIN(_sector_width_deg[_last_sector], 999), - MIN(_sector_middle_deg[_last_sector], 999)); + (unsigned int)PROXIMITY_SECTOR_WIDTH_DEG, + _sector_middle_deg[_last_sector]); uart->write(request_str); @@ -410,8 +349,8 @@ bool AP_Proximity_LightWareSF40C_v09::process_reply() { float angle_deg = (float)atof(element_buf[0]); float distance_m = (float)atof(element_buf[1]); - uint8_t sector; - if (convert_angle_to_sector(angle_deg, sector)) { + if (!ignore_reading(angle_deg)) { + const uint8_t sector = convert_angle_to_sector(angle_deg); _angle[sector] = angle_deg; _distance[sector] = distance_m; _distance_valid[sector] = is_positive(distance_m); diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h index a34f309710..d963f1d41f 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.h @@ -36,7 +36,6 @@ private: // initialise sensor (returns true if sensor is successfully initialised) bool initialise(); - void init_sectors(); void set_motor_speed(bool on_off); void set_motor_direction(); void set_forward_direction(); @@ -93,5 +92,4 @@ private: uint8_t _motor_speed; // motor speed as reported by lidar uint8_t _motor_direction = 99; // motor direction as reported by lidar int16_t _forward_direction = 999; // forward direction as reported by lidar - bool _sector_initialised = false; }; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index eb9e92e4f6..78e7f618a0 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -109,11 +109,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) const bool database_ready = database_prepare_for_push(current_loc, current_heading); // initialise updated array and proximity sector angles (to closest object) and distances - bool sector_updated[_num_sectors]; - float sector_width_half[_num_sectors]; - for (uint8_t i = 0; i < _num_sectors; i++) { + bool sector_updated[PROXIMITY_NUM_SECTORS]; + for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) { sector_updated[i] = false; - sector_width_half[i] = _sector_width_deg[i] * 0.5f; _angle[i] = _sector_middle_deg[i]; _distance[i] = MAX_DISTANCE; } @@ -134,10 +132,10 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) const float mid_angle = wrap_360((float)j * increment + yaw_correction); // iterate over proximity sectors - for (uint8_t i = 0; i < _num_sectors; i++) { + for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) { float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - mid_angle)); // update distance array sector with shortest distance from message - if ((angle_diff <= sector_width_half[i]) && (packet_distance_m < _distance[i])) { + if ((angle_diff <= (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) && (packet_distance_m < _distance[i])) { _distance[i] = packet_distance_m; _angle[i] = mid_angle; sector_updated[i] = true; @@ -151,7 +149,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) } // update proximity sectors validity and boundary point - for (uint8_t i = 0; i < _num_sectors; i++) { + for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) { _distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max); if (sector_updated[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 42f0c15444..2740360e55 100644 --- a/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MorseSITL.cpp @@ -41,10 +41,6 @@ void AP_Proximity_MorseSITL::update(void) memset(_angle, 0, sizeof(_angle)); memset(_distance, 0, sizeof(_distance)); - // only use 8 sectors to match RPLidar - const uint8_t nsectors = MIN(8, PROXIMITY_SECTORS_MAX); - const uint16_t degrees_per_sector = 360 / nsectors; - for (uint16_t i=0; i 0) && (sector < PROXIMITY_SECTORS_MAX)) { - uint16_t sector_size; - if (degrees_to_fill >= 90) { - // set sector to maximum of 45 degrees - sector_size = 45; - } else if (degrees_to_fill > 45) { - // use half the remaining area to optimise size of this sector and the next - sector_size = degrees_to_fill / 2.0f; - } else { - // 45 degrees or less are left so put it all into the next sector - sector_size = degrees_to_fill; - } - // record the sector middle and width - _sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f); - _sector_width_deg[sector] = sector_size; - - // move onto next sector - start_angle += sector_size; - sector++; - degrees_to_fill -= sector_size; - } - } - } - - // set num sectors - _num_sectors = sector; - - // re-initialise boundary because sector locations have changed - init_boundary(); - - // record success - _sector_initialised = true; -} - // set Lidar into SCAN mode void AP_Proximity_RPLidarA2::set_scan_mode() { @@ -403,8 +341,8 @@ void AP_Proximity_RPLidarA2::parse_response_data() Debug(2, " D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality); #endif _last_distance_received_ms = AP_HAL::millis(); - uint8_t sector; - if (convert_angle_to_sector(angle_deg, sector)) { + if (!ignore_reading(angle_deg)) { + const uint8_t sector = convert_angle_to_sector(angle_deg); if (distance_m > distance_min()) { if (_last_sector == sector) { if (_distance_m_last > distance_m) { diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index 26a2648701..85aae9bc9e 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h @@ -69,7 +69,6 @@ private: // initialise sensor (returns true if sensor is successfully initialised) bool initialise(); - void init_sectors(); void set_scan_mode(); // send request for something from sensor @@ -87,7 +86,6 @@ private: bool _information_data; bool _resetted; bool _initialised; - bool _sector_initialised; uint8_t _payload_length; uint8_t _cnt; diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 7d7d5006a7..77de21eb29 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -62,7 +62,7 @@ void AP_Proximity_SITL::update(void) _distance_valid[last_sector] = false; } last_sector++; - if (last_sector >= _num_sectors) { + if (last_sector >= PROXIMITY_NUM_SECTORS) { last_sector = 0; } } else { diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index bacf96a42b..227336261d 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -119,13 +119,11 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data() // process reply void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm) { - uint8_t sector; - if (convert_angle_to_sector(angle_deg, sector)) { - _angle[sector] = angle_deg; - _distance[sector] = ((float) distance_cm) / 1000; - _distance_valid[sector] = distance_cm != 0xffff; - _last_distance_received_ms = AP_HAL::millis(); - // update boundary used for avoidance - update_boundary_for_sector(sector, true); - } + const uint8_t sector = convert_angle_to_sector(angle_deg); + _angle[sector] = angle_deg; + _distance[sector] = ((float) distance_cm) / 1000; + _distance_valid[sector] = distance_cm != 0xffff; + _last_distance_received_ms = AP_HAL::millis(); + // update boundary used for avoidance + 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 011811da24..01250de225 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -168,15 +168,13 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data() // process reply void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm) { - uint8_t sector; - if (convert_angle_to_sector(angle_deg, sector)) { - _angle[sector] = angle_deg; - _distance[sector] = ((float) distance_cm) / 1000; + const uint8_t sector = convert_angle_to_sector(angle_deg); + _angle[sector] = angle_deg; + _distance[sector] = ((float) distance_cm) / 1000; - //check for target too far, target too close and sensor not connected - _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, true); - } + //check for target too far, target too close and sensor not connected + _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, true); }