fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 07:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence: can select gps instead of global position

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-24 11:14:15 +02:00
parent 3c10b78e20
commit 8262739b62
5 changed files with 89 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+22 -1
View File
@@ -63,7 +63,8 @@ Geofence::Geofence() :
_altitude_max(0),
_verticesCount(0),
_param_geofence_on(this, "ON"),
_param_altitude_mode(this, "ALTMODE")
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE")
{
/* Load initial params */
updateParams();
@@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
}
bool Geofence::inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(double)gps_position.alt * 1.0e-3);
}
} else {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position, baro_altitude_amsl);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
baro_altitude_amsl);
}
}
}
bool Geofence::inside(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
src/modules/navigator/geofence.h
+13 -1
View File
@@ -43,6 +43,7 @@
#include <uORB/topics/fence.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_combined.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
@@ -57,10 +58,16 @@ public:
/* Altitude mode, corresponding to the param GF_ALTMODE */
enum {
GF_ALT_MODE_GPS = 0,
GF_ALT_MODE_WGS84 = 0,
GF_ALT_MODE_AMSL = 1
};
/* Source, corresponding to the param GF_SOURCE */
enum {
GF_SOURCE_GLOBALPOS = 0,
GF_SOURCE_GPS = 1
};
/**
* Return whether system is inside geofence.
*
@@ -71,6 +78,8 @@ public:
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(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl);
int clearDm();
@@ -88,6 +97,8 @@ public:
bool isEmpty() {return _verticesCount == 0;}
int getAltitudeMode() { return _param_altitude_mode.get(); }
int getSource() { return _param_source.get(); }
private:
orb_advert_t _fence_pub; /**< publish fence topic */
@@ -100,6 +111,7 @@ private:
/* Params */
control::BlockParamInt _param_geofence_on;
control::BlockParamInt _param_altitude_mode;
control::BlockParamInt _param_source;
};
src/modules/navigator/geofence_params.c
+15 -2
View File
@@ -63,10 +63,23 @@ PARAM_DEFINE_INT32(GF_ON, 1);
* Geofence altitude mode
*
* Select which altitude reference should be used
* 0 = GPS, 1 = AMSL
* 0 = WGS84, 1 = AMSL
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ALTMODE, 1);
PARAM_DEFINE_INT32(GF_ALTMODE, 0);
/**
* Geofence source
*
* Select which position source should be used. Selecting GPS instead of global position makes sure that there is
* no dependence on the position estimator
* 0 = global position, 1 = GPS
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_SOURCE, 0);
src/modules/navigator/navigator.h
+9
View File
@@ -51,6 +51,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -129,6 +130,7 @@ public:
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
@@ -152,6 +154,7 @@ private:
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _gps_pos_sub; /**< gps position subscription */
int _sensor_combined_sub; /**< sensor combined subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
@@ -171,6 +174,7 @@ private:
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
vehicle_gps_position_s _gps_pos; /**< gps position */
sensor_combined_s _sensor_combined; /**< sensor values */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
@@ -216,6 +220,11 @@ private:
*/
void global_position_update();
/**
* Retrieve gps position
*/
void gps_position_update();
/**
* Retrieve sensor values
*/
src/modules/navigator/navigator_main.cpp
+30 -8
View File
@@ -100,6 +100,7 @@ Navigator::Navigator() :
_navigator_task(-1),
_mavlink_fd(-1),
_global_pos_sub(-1),
_gps_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_capabilities_sub(-1),
@@ -114,6 +115,7 @@ Navigator::Navigator() :
_vstatus{},
_control_mode{},
_global_pos{},
_gps_pos{},
_sensor_combined{},
_home_pos{},
_mission_item{},
@@ -187,6 +189,12 @@ Navigator::global_position_update()
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
void
Navigator::gps_position_update()
{
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
void
Navigator::sensor_combined_update()
{
@@ -263,6 +271,7 @@ Navigator::task_main()
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -277,6 +286,7 @@ Navigator::task_main()
vehicle_status_update();
vehicle_control_mode_update();
global_position_update();
gps_position_update();
sensor_combined_update();
home_position_update();
navigation_capabilities_update();
@@ -289,7 +299,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[7];
struct pollfd fds[8];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
@@ -306,6 +316,8 @@ Navigator::task_main()
fds[5].events = POLLIN;
fds[6].fd = _sensor_combined_sub;
fds[6].events = POLLIN;
fds[7].fd = _gps_pos_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@@ -330,6 +342,16 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
bool have_geofence_position_data = false;
/* gps updated */
if (fds[7].revents & POLLIN) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
}
/* sensors combined updated */
if (fds[6].revents & POLLIN) {
sensor_combined_update();
@@ -364,14 +386,14 @@ Navigator::task_main()
/* global position updated */
if (fds[0].revents & POLLIN) {
global_position_update();
/* Check geofence violation */
bool inside = false;
if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) {
inside = _geofence.inside(_global_pos);
} else {
inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter);
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
}
/* Check geofence violation */
if (have_geofence_position_data) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
if (!inside) {
/* inform other apps via the sp triplet */
_pos_sp_triplet.geofence_violated = true;