From c7966d56f5457be2eab4d6ee9cfadb7c3f22674b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) 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 #include #include +#include /* 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 -- cgit v1.2.3