Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Randy Mackay 2016-11-15 16:27:34 +09:00
parent 87dea46f5d
commit 26332251f5
6 changed files with 210 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AP_Proximity/AP_Proximity.cpp
View File

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

3
libraries/AP_Proximity/AP_Proximity.h
View File

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

51
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

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

5
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

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

62
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
View File

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