dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

geofence: better usefeedback if loaded

This commit is contained in:
Thomas Gubler 2014-09-20 08:44:30 +02:00
parent 6962c5eedf
commit 2148464a70
3 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/navigator/geofence.cpp
View File

@ -48,6 +48,7 @@
#include <ctype.h> #include <ctype.h>
#include <nuttx/config.h> #include <nuttx/config.h>
#include <unistd.h> #include <unistd.h>
#include <mavlink/mavlink_log.h>
/* Oddly, ERROR is not defined for C++ */ /* Oddly, ERROR is not defined for C++ */
@ -66,7 +67,8 @@ Geofence::Geofence() :
_param_altitude_mode(this, "ALTMODE"), _param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"), _param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"), _param_counter_threshold(this, "COUNT"),
_outside_counter(0) _outside_counter(0),
_mavlinkFd(-1)
{ {
/* Load initial params */ /* Load initial params */
updateParams(); updateParams();
@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename)
{ {
_verticesCount = pointCounter; _verticesCount = pointCounter;
warnx("Geofence: imported successfully"); warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
} else { } else {
warnx("Geofence: import error"); warnx("Geofence: import error");
mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
} }
return ERROR; return ERROR;

4
src/modules/navigator/geofence.h
View File

@ -98,6 +98,8 @@ public:
int getSource() { return _param_source.get(); } int getSource() { return _param_source.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private: private:
orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _fence_pub; /**< publish fence topic */
@ -114,6 +116,8 @@ private:
uint8_t _outside_counter; uint8_t _outside_counter;
int _mavlinkFd;
bool inside(double lat, double lon, float altitude); 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);
bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);

2
src/modules/navigator/navigator_main.cpp
View File

@ -252,6 +252,7 @@ Navigator::task_main()
warnx("Initializing.."); warnx("Initializing..");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
/* Try to load the geofence: /* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file * if /fs/microsd/etc/geofence.txt load from this file
@ -263,6 +264,7 @@ Navigator::task_main()
_geofence.loadFromFile(GEOFENCE_FILENAME); _geofence.loadFromFile(GEOFENCE_FILENAME);
} else { } else {
mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
if (_geofence.clearDm() > 0) if (_geofence.clearDm() > 0)
warnx("Geofence cleared"); warnx("Geofence cleared");
else else