Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: cope with polyfence holding boundary points

Browse Source
This commit is contained in:
Peter Barker 2019-05-22 15:03:47 +10:00 committed by Randy Mackay
parent ff37590776
commit 5fe16d5250
2 changed files with 9 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -18,6 +18,7 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include "AP_Proximity_SITL.h" #include "AP_Proximity_SITL.h"
#include <AC_Fence/AC_Fence.h>
#include <stdio.h> #include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -34,10 +35,6 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
sitl(AP::sitl()) sitl(AP::sitl())
{ {
ap_var_type ptype; ap_var_type ptype;
fence_count = (AP_Int8 *)AP_Param::find("FENCE_TOTAL", &ptype);
if (fence_count == nullptr || ptype != AP_PARAM_INT8) {
AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL");
}
fence_alt_max = (AP_Float *)AP_Param::find("FENCE_ALT_MAX", &ptype); fence_alt_max = (AP_Float *)AP_Param::find("FENCE_ALT_MAX", &ptype);
if (fence_alt_max == nullptr || ptype != AP_PARAM_FLOAT) { if (fence_alt_max == nullptr || ptype != AP_PARAM_FLOAT) {
AP_HAL::panic("Proximity_SITL: Failed to find FENCE_ALT_MAX"); AP_HAL::panic("Proximity_SITL: Failed to find FENCE_ALT_MAX");
@ -47,11 +44,14 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
// update the state of the sensor // update the state of the sensor
void AP_Proximity_SITL::update(void) void AP_Proximity_SITL::update(void)
{ {
load_fence();
current_loc.lat = sitl->state.latitude * 1.0e7; current_loc.lat = sitl->state.latitude * 1.0e7;
current_loc.lng = sitl->state.longitude * 1.0e7; current_loc.lng = sitl->state.longitude * 1.0e7;
current_loc.alt = sitl->state.altitude * 1.0e2; current_loc.alt = sitl->state.altitude * 1.0e2;
if (fence && fence_loader.boundary_valid(fence_count->get(), fence)) {
if (!AP::fence()->polyfence().breached()) {
// only called to prompt polyfence to reload fence if required
}
if (AP::fence()->polyfence().valid()) {
// update distance in one sector // update distance in one sector
if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) { if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) {
set_status(AP_Proximity::Proximity_Good); set_status(AP_Proximity::Proximity_Good);
@ -70,29 +70,10 @@ void AP_Proximity_SITL::update(void)
} }
} }
void AP_Proximity_SITL::load_fence(void)
{
uint32_t now = AP_HAL::millis();
if (now - last_load_ms < 1000) {
return;
}
last_load_ms = now;
if (fence == nullptr) {
fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l));
}
if (fence == nullptr) {
return;
}
for (uint8_t i=0; i<fence_count->get(); i++) {
fence_loader.load_point_from_eeprom(i, fence[i]);
}
}
// get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction) // get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance) const bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance) const
{ {
if (!fence_loader.boundary_valid(fence_count->get(), fence)) { if (!AP::fence()->polyfence().valid()) {
return false; return false;
} }
@ -106,10 +87,10 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance)
float min_dist = 0, max_dist = PROXIMITY_MAX_RANGE; float min_dist = 0, max_dist = PROXIMITY_MAX_RANGE;
while (max_dist - min_dist > PROXIMITY_ACCURACY) { while (max_dist - min_dist > PROXIMITY_ACCURACY) {
float test_dist = (max_dist+min_dist)*0.5f; float test_dist = (max_dist+min_dist)*0.5f;
Location loc = current_loc; Location loc = current_loc;
loc.offset_bearing(angle_deg, test_dist); loc.offset_bearing(angle_deg, test_dist);
Vector2l vecloc(loc.lat, loc.lng); if (AP::fence()->polyfence().breached(loc)) {
if (fence_loader.boundary_breached(vecloc, fence_count->get(), fence)) {
max_dist = test_dist; max_dist = test_dist;
} else { } else {
min_dist = test_dist; min_dist = test_dist;

6
libraries/AP_Proximity/AP_Proximity_SITL.h
View File

@ -26,18 +26,12 @@ public:
private: private:
SITL::SITL *sitl; SITL::SITL *sitl;
Vector2l *fence;
AP_Int8 *fence_count;
AP_Float *fence_alt_max; AP_Float *fence_alt_max;
uint32_t last_load_ms;
AC_PolyFence_loader fence_loader;
Location current_loc; Location current_loc;
// latest sector updated // latest sector updated
uint8_t last_sector; uint8_t last_sector;
void load_fence(void);
// get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction) // get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool get_distance_to_fence(float angle_deg, float &distance) const; bool get_distance_to_fence(float angle_deg, float &distance) const;