Skip to content

Commit

Permalink
geofence max horz/vertical better messages
Browse files Browse the repository at this point in the history
  • Loading branch information
dagar authored and LorenzMeier committed Nov 19, 2016
1 parent c701085 commit 6cdd188
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 8 deletions.
18 changes: 10 additions & 8 deletions src/modules/navigator/geofence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -131,42 +131,44 @@ bool Geofence::inside(const struct mission_item_s &mission_item)

bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = true;

float max_horizontal_distance = _param_max_hor_distance.get();
float max_vertical_distance = _param_max_ver_distance.get();

if (max_horizontal_distance > 1 || max_vertical_distance > 1) {
if (max_horizontal_distance > 1.0f || max_vertical_distance > 1.0f) {
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)) {
if (max_vertical_distance > 1.0f && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max vertical distance by %.1f m",
"Maximum altitude above home exceeded by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}

return false;
inside_fence = false;
}

if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (max_horizontal_distance > 1.0f && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max horizontal distance by %.1f m",
"Maximum distance from home exceeded by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}

return false;
inside_fence = false;
}
}
}

bool inside_fence = inside_polygon(lat, lon, altitude);
inside_fence |= inside_polygon(lat, lon, altitude);

if (inside_fence) {
_outside_counter = 0;
Expand Down
1 change: 1 addition & 0 deletions src/modules/navigator/geofence_params.c
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@
* @value 1 Warning
* @value 2 Loiter
* @value 3 Return to Land
* @value 4 Flight terminate
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ACTION, 1);
Expand Down

0 comments on commit 6cdd188

Please sign in to comment.