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

navigator: don't publish mission result immediately but only after every iteration of the navigator

This commit is contained in:
Ban Siesta 2014-12-19 23:36:32 +00:00
parent c9ca61ef5b
commit f0ff914b62
7 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/navigator/datalinkloss.cpp
View File

@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
warnx("time is up, state should have been changed manually by now");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:

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

@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
warnx("gps fail: request flight termination");
}
default:

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

@ -711,7 +711,7 @@ Mission::set_mission_item_reached()
{
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
}
@ -721,7 +721,7 @@ Mission::set_current_offboard_mission_item()
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
save_offboard_mission_state();
}
@ -730,5 +730,5 @@ void
Mission::set_mission_finished()
{
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
}

13
src/modules/navigator/navigator.h
View File

@ -107,11 +107,6 @@ 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();
/**
* Publish the geofence result
*/
@ -128,6 +123,7 @@ public:
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
void set_mission_result_updated() { _mission_result_updated = true; }
/**
* Getters
@ -215,6 +211,7 @@ private:
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated; /**< flags if mission result has seen an update */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
@ -280,6 +277,12 @@ private:
*/
void publish_position_setpoint_triplet();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/

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

@ -139,6 +139,7 @@ Navigator::Navigator() :
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_pos_sp_triplet_published_invalid_once(false),
_mission_result_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@ -491,6 +492,11 @@ Navigator::task_main()
_pos_sp_triplet_updated = false;
}
if (_mission_result_updated) {
publish_mission_result();
_mission_result_updated = false;
}
perf_end(_loop_perf);
}
warnx("exiting.");

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

@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
on_activation();
} else {

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

@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@ -162,7 +162,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
}
break;
@ -171,7 +171,7 @@ RCLoss::advance_rcl()
warnx("time is up, no RC regain, terminating");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE: