From 26332251f5288aad667ac1dcbeb56740d8df3860 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 15 Nov 2016 16:27:34 +0900 Subject: [PATCH] AP_Proximity: add ignore areas Only SF40c uses these ignore areas for now at least. It is safe to increase the eeprom locations for the 2nd proximity instance because we only define a single instance. --- libraries/AP_Proximity/AP_Proximity.cpp | 90 ++++++++++++++++++- libraries/AP_Proximity/AP_Proximity.h | 3 + .../AP_Proximity/AP_Proximity_Backend.cpp | 51 +++++++++++ libraries/AP_Proximity/AP_Proximity_Backend.h | 5 ++ .../AP_Proximity_LightWareSF40C.cpp | 62 +++++++++++++ .../AP_Proximity_LightWareSF40C.h | 2 + 6 files changed, 210 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index d852a2616b..313c151588 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -44,27 +44,111 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @User: Standard AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], PROXIMITY_YAW_CORRECTION_DEFAULT), + // @Param: _IGN_ANG1 + // @DisplayName: Proximity sensor ignore angle 1 + // @Description: Proximity sensor ignore angle 1 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0), + + // @Param: _IGN_WID1 + // @DisplayName: Proximity sensor ignore width 1 + // @Description: Proximity sensor ignore width 1 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0), + + // @Param: _IGN_ANG2 + // @DisplayName: Proximity sensor ignore angle 2 + // @Description: Proximity sensor ignore angle 2 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0), + + // @Param: _IGN_WID1 + // @DisplayName: Proximity sensor ignore width 2 + // @Description: Proximity sensor ignore width 2 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0), + + // @Param: _IGN_ANG3 + // @DisplayName: Proximity sensor ignore angle 3 + // @Description: Proximity sensor ignore angle 3 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0), + + // @Param: _IGN_WID3 + // @DisplayName: Proximity sensor ignore width 3 + // @Description: Proximity sensor ignore width 3 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0), + + // @Param: _IGN_ANG4 + // @DisplayName: Proximity sensor ignore angle 4 + // @Description: Proximity sensor ignore angle 4 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0), + + // @Param: _IGN_WID4 + // @DisplayName: Proximity sensor ignore width 4 + // @Description: Proximity sensor ignore width 4 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0), + + // @Param: _IGN_ANG5 + // @DisplayName: Proximity sensor ignore angle 5 + // @Description: Proximity sensor ignore angle 5 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0), + + // @Param: _IGN_WID5 + // @DisplayName: Proximity sensor ignore width 5 + // @Description: Proximity sensor ignore width 5 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0), + + // @Param: _IGN_ANG6 + // @DisplayName: Proximity sensor ignore angle 6 + // @Description: Proximity sensor ignore angle 6 + // @Range: 0 360 + // @User: Standard + AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0), + + // @Param: _IGN_WID6 + // @DisplayName: Proximity sensor ignore width 6 + // @Description: Proximity sensor ignore width 6 + // @Range: 0 45 + // @User: Standard + AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0), + #if PROXIMITY_MAX_INSTANCES > 1 // @Param: 2_TYPE // @DisplayName: Second Proximity type // @Description: What type of proximity sensor is connected // @Values: 0:None,1:LightWareSF40C // @User: Advanced - AP_GROUPINFO("2_TYPE", 4, AP_Proximity, _type[1], 0), + AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0), // @Param: _ORIENT // @DisplayName: Second Proximity sensor orientation // @Description: Second Proximity sensor orientation // @Values: 0:Default,1:Upside Down // @User: Standard - AP_GROUPINFO("2_ORIENT", 5, AP_Proximity, _orientation[1], 0), + AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0), // @Param: _YAW_CORR // @DisplayName: Second Proximity sensor yaw correction // @Description: Second Proximity sensor yaw correction // @Range: -180 180 // @User: Standard - AP_GROUPINFO("2_YAW_CORR", 6, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT), + AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT), #endif AP_GROUPEND diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index db09425cf0..ce9838c064 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -23,6 +23,7 @@ #define PROXIMITY_MAX_INSTANCES 1 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_YAW_CORRECTION_DEFAULT 22 // default correction for sensor error in yaw +#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored class AP_Proximity_Backend; @@ -94,6 +95,8 @@ private: AP_Int8 _type[PROXIMITY_MAX_INSTANCES]; AP_Int8 _orientation[PROXIMITY_MAX_INSTANCES]; AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES]; + AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up) + AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored void detect_instance(uint8_t instance); void update_instance(uint8_t instance); diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 2563adfcd2..26889f885c 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -112,3 +112,54 @@ bool AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees, uint8_t return false; } + +// get ignore area info +uint8_t AP_Proximity_Backend::get_ignore_area_count() const +{ + // count number of ignore sectors + uint8_t count = 0; + 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 (found) { + ignore_start = smallest_angle_start; + } + return found; +} diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 6ff3744586..480ab9f38c 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -49,6 +49,11 @@ protected: // find which sector a given angle falls into bool convert_angle_to_sector(float angle_degrees, uint8_t §or) const; + // 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; + 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 d2b1b18cb1..67b049b43e 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -91,9 +91,71 @@ bool AP_Proximity_LightWareSF40C::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::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; + + // record success + _sector_initialised = true; +} + // set speed of rotating motor void AP_Proximity_LightWareSF40C::set_motor_speed(bool on_off) { diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index 17d5641b7e..1cfb6ba958 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h @@ -31,6 +31,7 @@ private: // initialise sensor (returns true if sensor is succesfully initialised) bool initialise(); + void init_sectors(); void set_motor_speed(bool on_off); void set_motor_direction(); void set_forward_direction(); @@ -87,4 +88,5 @@ 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; };