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

navigator geofence: switch to new dataman data structure, support multiple polygons

Browse Source 
This also removes the 'navigator fence <lon> <lat>' command to simplify
code (I don't think there's still use for that anymore). However the
file loading is still supported.

If goefence.txt does not exist, navigator will not clear the geofence
anymore on startup.
This commit is contained in:
Beat Küng
2017-08-04 18:08:51 +02:00
committed by Lorenz Meier
parent a987886ca3
commit 328e84117e
6 changed files with 315 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+16 -24
View File
@@ -226,18 +226,12 @@ Navigator::task_main()
bool have_geofence_position_data = false;
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager */
* if /fs/microsd/etc/geofence.txt load from this file */
struct stat buffer;
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
PX4_INFO("Try to load geofence.txt");
PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() != OK) {
mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
}
}
/* do subscriptions */
@@ -532,7 +526,8 @@ Navigator::task_main()
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
bool inside = _geofence.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
@@ -701,12 +696,18 @@ Navigator::start()
void
Navigator::status()
{
if (_geofence.valid()) {
PX4_INFO("Geofence is valid");
/* TODO: add this again */
// PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in");
// if (_global_pos.global_valid) {
// PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
// PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters",
// (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
// PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
// (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
// PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
// }
} else {
PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
}
}
void
@@ -819,12 +820,6 @@ Navigator::get_acceptance_radius(float mission_item_radius)
return radius;
}
void
Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);
}
void
Navigator::load_fence_from_file(const char *filename)
{
@@ -852,7 +847,7 @@ Navigator::abort_landing()
static void usage()
{
PX4_INFO("usage: navigator {start|stop|status|fence|fencefile}");
PX4_INFO("usage: navigator {start|stop|status|fencefile}");
}
int navigator_main(int argc, char *argv[])
@@ -898,9 +893,6 @@ int navigator_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "status")) {
navigator::g_navigator->status();
} else if (!strcmp(argv[1], "fence")) {
navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
} else if (!strcmp(argv[1], "fencefile")) {
navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);