From 3143f6bd0acde35601de5b8fff3fc2796c1a2104 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Wed, 10 May 2023 17:43:46 +0200 Subject: [PATCH] mavlink_mission & mission: move dataman update to mission --- src/modules/mavlink/mavlink_mission.cpp | 59 ++++++------------------- src/modules/mavlink/mavlink_mission.h | 2 +- src/modules/navigator/mission.cpp | 8 ++++ 3 files changed, 23 insertions(+), 46 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 9a95154023..f71523b500 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -138,44 +138,22 @@ MavlinkMissionManager::load_safepoint_stats() /** * Publish mission topic to notify navigator about changes. */ -int +void MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) { - // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; mission.timestamp = hrt_absolute_time(); mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; - /* update mission state in dataman */ + /* update active mission state */ + _dataman_id = dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = count; + _current_seq = seq; + _my_dataman_id = _dataman_id; - bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), - sizeof(mission_s)); - - if (success) { - /* update active mission state */ - _dataman_id = dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = count; - _current_seq = seq; - _my_dataman_id = _dataman_id; - - /* mission state saved successfully, publish offboard_mission topic */ - _offboard_mission_pub.publish(mission); - - return PX4_OK; - - } else { - PX4_ERR("WPM: can't save mission state"); - - if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); - events::send(events::ID("mavlink_mission_storage_write_failure"), events::Log::Critical, - "Mission: Unable to write to storage"); - } - - return PX4_ERROR; - } + _offboard_mission_pub.publish(mission); } int @@ -659,16 +637,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { - if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); - - } else { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); - - _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID\t"); - events::send(events::ID("mavlink_mission_err_id"), events::Log::Error, - "Failed to write current mission ID to storage"); - } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq); } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); @@ -1182,7 +1151,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); break; case MAV_MISSION_TYPE_FENCE: @@ -1236,8 +1205,8 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; case MAV_MISSION_TYPE_FENCE: @@ -1249,9 +1218,9 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_ALL: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); - ret = update_geofence_count(0) || ret; + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ret = update_geofence_count(0); ret = update_safepoint_count(0) || ret; break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index c9eec1e882..ae8407b038 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -160,7 +160,7 @@ private: void init_offboard_mission(); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1c56fc8f87..dd2d85ed88 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -595,6 +595,14 @@ Mission::update_mission() const mission_s old_mission = _mission; if (_mission_sub.copy(&_mission)) { + + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), + sizeof(mission_s)); + + if (!success) { + PX4_ERR("Can't update mission state in Dataman"); + } + /* determine current index */ if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { _current_mission_index = _mission.current_seq;