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

topics: move geofence status to its own topic

Browse Source
This commit is contained in:
Ban Siesta
2014-12-19 22:26:31 +00:00
parent 19d5383c56
commit 2a09473e5f
6 changed files with 137 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator.h
+10 -1
View File
@@ -54,6 +54,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include "navigator_mode.h"
@@ -111,6 +112,11 @@ public:
*/
void publish_mission_result();
/**
* Publish the geofence result
*/
void publish_geofence_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
* Example: mode that is triggered on gps failure
@@ -134,6 +140,7 @@ public:
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +171,7 @@ private:
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
orb_advert_t _geofence_result_pub;
orb_advert_t _att_sp_pub; /**< publish att sp
used only in very special failsafe modes
when pos control is deactivated */
@@ -179,7 +187,8 @@ private:
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
mission_result_s _mission_result;
vehicle_attitude_setpoint_s _att_sp;
geofence_result_s _geofence_result;
vehicle_attitude_setpoint_s _att_sp;
bool _mission_item_valid; /**< flags if the current mission item is valid */
src/modules/navigator/navigator_main.cpp
+20 -4
View File
@@ -110,6 +110,7 @@ Navigator::Navigator() :
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_geofence_result_pub(-1),
_att_sp_pub(-1),
_vstatus{},
_control_mode{},
@@ -398,8 +399,8 @@ Navigator::task_main()
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
publish_mission_result();
_geofence_result.geofence_violated = true;
publish_geofence_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -408,8 +409,8 @@ Navigator::task_main()
}
} else {
/* inform other apps via the mission result */
_mission_result.geofence_violated = false;
publish_mission_result();
_geofence_result.geofence_violated = false;
publish_geofence_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -641,6 +642,21 @@ Navigator::publish_mission_result()
}
}
void
Navigator::publish_geofence_result()
{
/* lazily publish the geofence result only once available */
if (_geofence_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
} else {
/* advertise and publish */
_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
}
}
void
Navigator::publish_att_sp()
{