Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

geofence max horz/vertical better messages

Browse Source
This commit is contained in:
Daniel Agar
2016-11-06 15:00:43 -05:00
committed by Lorenz Meier
parent c701085ed4
commit 6cdd188158
2 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+10 -8
View File
@@ -131,10 +131,12 @@ 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;
@@ -142,31 +144,31 @@ bool Geofence::inside(double lat, double lon, float 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;
src/modules/navigator/geofence_params.c
+1
View File
@@ -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);