diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-20 08:44:30 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-22 10:54:51 +0200 |
commit | c7966d56f5457be2eab4d6ee9cfadb7c3f22674b (patch) | |
tree | 6d4f566608a82e62c8a89ec59c81c84654b286da | |
parent | b5ffcfe3d12a0e2e98cb5319e0286c8215150672 (diff) | |
download | px4-firmware-c7966d56f5457be2eab4d6ee9cfadb7c3f22674b.tar.gz px4-firmware-c7966d56f5457be2eab4d6ee9cfadb7c3f22674b.tar.bz2 px4-firmware-c7966d56f5457be2eab4d6ee9cfadb7c3f22674b.zip |
geofence: better usefeedback if loaded
-rw-r--r-- | src/modules/navigator/geofence.cpp | 6 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 4 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 |
3 files changed, 11 insertions, 1 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5..0f431ded2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include <ctype.h> #include <nuttx/config.h> #include <unistd.h> +#include <mavlink/mavlink_log.h> /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042..9d647cb68 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc9120f13..4f92954fd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else |