fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:29:07 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/commander/commander.cpp
View File

@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[])
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {
if (telemetry_lost[i]) {//XXX also add hysteresis here
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
}
@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished);
mission_result.finished,
mission_result.stay_in_failsafe);
// TODO handle mode changes by commands
if (main_state_changed) {

3
src/modules/commander/state_machine_helper.cpp
View File

@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;

2
src/modules/commander/state_machine_helper.h
View File

@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);

2
src/modules/navigator/datalinkloss.cpp
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;

32
src/modules/navigator/mission.cpp
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;
}

8
src/modules/navigator/mission.h
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,

13
src/modules/navigator/navigator.h
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 */

22
src/modules/navigator/navigator_main.cpp
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;
}

3
src/modules/navigator/navigator_mode.cpp
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 {

1
src/modules/uORB/topics/mission_result.h
View File

@ -57,6 +57,7 @@ struct mission_result_s
unsigned seq_current; /**< Sequence of the current mission item */
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
};
/**