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

add method to block fallback to mission

Browse Source 
failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission
This commit is contained in:
Thomas Gubler
2014-07-23 22:58:19 +02:00
parent ed19faf428
commit 24f380137e
10 changed files with 53 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/datalinkloss.cpp
+2
View File
@@ -200,6 +200,8 @@ DataLinkLoss::advance_dll()
case DLL_STATE_FLYTOCOMMSHOLDWP:
//XXX check here if time is over are over
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
break;
default:
break;
src/modules/navigator/mission.cpp
+7 -25
View File
@@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff(false),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
@@ -577,18 +575,18 @@ void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
_navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
save_offboard_mission_state();
}
@@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item()
void
Mission::report_mission_finished()
{
_mission_result.finished = true;
publish_mission_result();
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
}
src/modules/navigator/mission.h
-8
View File
@@ -128,11 +128,6 @@ private:
*/
void report_mission_finished();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
@@ -145,9 +140,6 @@ private:
bool _need_takeoff;
bool _takeoff;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
src/modules/navigator/navigator.h
+12 -1
View File
@@ -52,6 +52,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mode.h"
#include "mission.h"
@@ -102,6 +103,11 @@ public:
*/
void load_fence_from_file(const char *filename);
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
/**
* Setters
*/
@@ -115,7 +121,9 @@ public:
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
@@ -143,6 +151,7 @@ private:
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -152,6 +161,8 @@ private:
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
mission_result_s _mission_result;
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
src/modules/navigator/navigator_main.cpp
+22
View File
@@ -108,6 +108,7 @@ Navigator::Navigator() :
_offboard_mission_sub(-1),
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_vstatus{},
_control_mode{},
_global_pos{},
@@ -115,6 +116,7 @@ Navigator::Navigator() :
_mission_item{},
_nav_caps{},
_pos_sp_triplet{},
_mission_result{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
@@ -370,6 +372,9 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* Some failsafe modes prohibit the fallback to mission
* usually this is done after some time to make sure
* that the full failsafe operation is performed */
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
@@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[])
return 0;
}
void
Navigator::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
}
src/modules/navigator/navigator_mode.cpp
+3
View File
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
if (_first_run) {
/* first run */
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
on_activation();
} else {