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

@@ -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

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);  

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;
+}

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 &sector) 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
 

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<ignore_area_count; i++) {
+
+        // get ignore area info
+        uint16_t ign_area_angle;
+        uint8_t ign_area_width;
+        if (get_ignore_area(i, ign_area_angle, ign_area_width)) {
+
+            // calculate how many degrees of space we have between this end of this ignore area and the start of the end
+            int16_t start_angle, end_angle;
+            get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
+            get_next_ignore_start_or_end(0, start_angle, end_angle);
+            int16_t degrees_to_fill = wrap_360(end_angle - start_angle);
+
+            // divide up the area into sectors
+            while ((degrees_to_fill > 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)
 {

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;
 };