Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 6 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 Geofence::inside(double lat, double lon, float altitude)
{ {
bool inside_fence = true;
float max_horizontal_distance = _param_max_hor_distance.get(); float max_horizontal_distance = _param_max_hor_distance.get();
float max_vertical_distance = _param_max_ver_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) { if (_home_pos_set) {
float dist_xy = -1.0f; float dist_xy = -1.0f;
float dist_z = -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, _home_pos.lat, _home_pos.lon, _home_pos.alt,
&dist_xy, &dist_z); &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) { if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), 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)); (double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time(); _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) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), 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)); (double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time(); _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) { if (inside_fence) {
_outside_counter = 0; _outside_counter = 0;
src/modules/navigator/geofence_params.c
+1
View File
@@ -58,6 +58,7 @@
* @value 1 Warning * @value 1 Warning
* @value 2 Loiter * @value 2 Loiter
* @value 3 Return to Land * @value 3 Return to Land
* @value 4 Flight terminate
* @group Geofence * @group Geofence
*/ */
PARAM_DEFINE_INT32(GF_ACTION, 1); PARAM_DEFINE_INT32(GF_ACTION, 1);