diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-06-13 23:40:48 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-06-13 23:40:48 +0200 |
commit | 91b590ef584cfc67be7555e3d7272bb94bc9b2b4 (patch) | |
tree | b83457c6ff1dc496dd3a2074aa003ff711f9a169 /src/modules/navigator/mission.cpp | |
parent | d84cbd008c80aab8057fbdef94f57073e437ec56 (diff) | |
download | px4-firmware-91b590ef584cfc67be7555e3d7272bb94bc9b2b4.tar.gz px4-firmware-91b590ef584cfc67be7555e3d7272bb94bc9b2b4.tar.bz2 px4-firmware-91b590ef584cfc67be7555e3d7272bb94bc9b2b4.zip |
Move MISSION_STATE read/write from mavlink to navigator and commander
Diffstat (limited to 'src/modules/navigator/mission.cpp')
-rw-r--r-- | src/modules/navigator/mission.cpp | 33 |
1 files changed, 33 insertions, 0 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..d9d8353f6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -431,6 +431,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } 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)); + + /* check if state actually changed to save flash write cycles */ + if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || + mission_state.count != _offboard_mission.count || + mission_state.current_index != _current_offboard_mission_index) { + + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_index = _current_offboard_mission_index; + + /* write modifyed 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)) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + +void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { @@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void |