fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 03:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Geofence max horizontal and vertical distance

Browse Source 
-changes GF_ON to GF_MODE
-adds GF_MAX_HOR_DIST and GF_MAX_VER_DIST params
This commit is contained in:
Daniel Agar
2015-02-22 23:04:23 -05:00
parent 3c36a61569
commit 25dfd84b40
5 changed files with 91 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+48 -5
View File
@@ -39,6 +39,7 @@
*/
#include "geofence.h"
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <string.h>
#include <dataman/dataman.h>
@@ -49,6 +50,12 @@
#include <nuttx/config.h>
#include <unistd.h>
#include <mavlink/mavlink_log.h>
#include <geo/geo.h>
#define GEOFENCE_OFF 0
#define GEOFENCE_FILE_ONLY 1
#define GEOFENCE_MAX_DISTANCES_ONLY 2
#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
/* Oddly, ERROR is not defined for C++ */
@@ -60,13 +67,17 @@ static const int ERROR = -1;
Geofence::Geofence() :
SuperBlock(NULL, "GF"),
_fence_pub(-1),
_home_pos{},
_home_pos_set(false),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
_param_geofence_on(this, "ON"),
_param_geofence_mode(this, "MODE"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
_param_max_hor_distance(this, "MAX_HOR_DIST"),
_param_max_ver_distance(this, "MAX_VER_DIST"),
_outside_counter(0),
_mavlinkFd(-1)
{
@@ -92,10 +103,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
bool Geofence::inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
const struct home_position_s home_pos, bool home_position_set)
{
updateParams();
_home_pos = home_pos;
_home_pos_set = home_position_set;
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position);
@@ -118,13 +133,40 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
bool Geofence::inside(double lat, double lon, float altitude)
{
if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
int32_t max_horizontal_distance = _param_max_hor_distance.get();
int32_t max_vertical_distance = _param_max_ver_distance.get();
if (max_horizontal_distance > 0 || max_vertical_distance > 0) {
if (_home_pos_set) {
float dist_xy = -1.0f;
float dist_z = -1.0f;
get_distance_to_point_global_wgs84(lat, lon, altitude,
_home_pos.lat, _home_pos.lon, _home_pos.alt,
&dist_xy, &dist_z);
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max vertical distance by %.0f m",
(double)(dist_z - max_vertical_distance));
return false;
}
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max horizontal distance by %.0f m",
(double)(dist_xy - max_horizontal_distance));
return false;
}
}
}
}
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} {
} else {
_outside_counter++;
if (_outside_counter > _param_counter_threshold.get()) {
@@ -139,8 +181,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (_param_geofence_on.get() != 1) {
/* Return true if geofence is disabled or only checking max distances */
if ((_param_geofence_mode.get() == GEOFENCE_OFF)
|| (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
return true;
}