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

Merge branch 'dataman_state_nav_rewrite' into follower2

Browse Source
This commit is contained in:
Anton Babushkin
2014-07-04 17:29:20 +02:00
parent 829a317d23 39dab45ac5
commit 3f307fa0d7
17 changed files with 1271 additions and 1001 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+83 -31
View File
@@ -68,14 +68,12 @@ Mission::Mission(Navigator *navigator, const char *name) :
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_need_takeoff(true),
_takeoff(false)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
Mission::~Mission()
@@ -85,16 +83,25 @@ Mission::~Mission()
void
Mission::on_inactive()
{
/* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
if (_inited) {
/* check if missions have changed so that feedback to ground station is given */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
} else {
/* read mission topics on initialization */
_inited = true;
update_onboard_mission();
update_offboard_mission();
}
@@ -113,13 +120,13 @@ void
Mission::on_active()
{
/* check if anything has changed */
bool onboard_updated;
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
@@ -148,9 +155,9 @@ Mission::update_onboard_mission()
{
if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
/* accept the current index set by the onboard mission if it is within bounds */
if (_onboard_mission.current_index >=0
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_index;
if (_onboard_mission.current_seq >=0
&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_seq;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
@@ -164,7 +171,7 @@ Mission::update_onboard_mission()
} else {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
_onboard_mission.current_seq = 0;
_current_onboard_mission_index = 0;
}
}
@@ -173,14 +180,12 @@ void
Mission::update_offboard_mission()
{
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
/* determine current index */
if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index;
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
} else {
/* if less WPs available, reset to first WP */
/* if less items available, reset to first item */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
@@ -208,8 +213,9 @@ Mission::update_offboard_mission()
_navigator->get_home_position()->alt);
} else {
warnx("offboard mission update failed");
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
}
@@ -472,12 +478,55 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
return false;
}
void
Mission::save_offboard_mission_state()
{
mission_s mission_state;
/* lock MISSION_STATE item */
dm_lock(DM_KEY_MISSION_STATE);
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
if (read_res == sizeof(mission_s)) {
/* data read successfully, check dataman ID and items count */
if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
/* navigator may modify only sequence, write modified state only if it changed */
if (mission_state.current_seq != _current_offboard_mission_index) {
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
}
}
}
} else {
/* invalid data, this must not happen and indicates error in offboard_mission publisher */
mission_state.dataman_id = _offboard_mission.dataman_id;
mission_state.count = _offboard_mission.count;
mission_state.current_seq = _current_offboard_mission_index;
warnx("ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
/* write modified state only if changed */
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
}
}
/* unlock MISSION_STATE item */
dm_unlock(DM_KEY_MISSION_STATE);
}
void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
}
@@ -485,14 +534,17 @@ Mission::report_mission_item_reached()
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
save_offboard_mission_state();
}
void
Mission::report_mission_finished()
{
_mission_result.mission_finished = true;
_mission_result.finished = true;
publish_mission_result();
}
@@ -509,6 +561,6 @@ Mission::publish_mission_result()
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
_mission_result.mission_finished = false;
_mission_result.reached = false;
_mission_result.finished = false;
}