diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 16eab8645a..38e61755ed 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -705,9 +705,6 @@ Commander::Commander() : // default for vtol is rotary wing _vtol_status.vtol_in_rw_mode = true; - - /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ - mission_init(); } bool @@ -3401,31 +3398,6 @@ void Commander::enable_hil() _status.hil_state = vehicle_status_s::HIL_STATE_ON; } -void Commander::mission_init() -{ - /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ - mission_s mission; - - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { - if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { - if (mission.count > 0) { - PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs, curr: %" PRId32, mission.dataman_id, mission.count, - mission.current_seq); - } - - } else { - PX4_ERR("reading mission state failed"); - - /* initialize mission state in dataman */ - mission.timestamp = hrt_absolute_time(); - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); - } - - _mission_pub.publish(mission); - } -} - void Commander::data_link_check() { for (auto &telemetry_status : _telemetry_status_subs) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 97348e0efd..c89a26f652 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -72,7 +72,6 @@ #include #include #include -#include #include #include #include @@ -154,8 +153,6 @@ private: void executeActionRequest(const action_request_s &action_request); - void mission_init(); - void offboard_control_update(); void print_reject_mode(uint8_t main_state); @@ -444,7 +441,6 @@ private: uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)}; - uORB::Publication _mission_pub{ORB_ID(mission)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index 82d9041cd6..a9266d76d7 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -60,28 +60,23 @@ __END_DECLS static constexpr int TASK_STACK_SIZE = 1220; /* Private File based Operations */ -static ssize_t _file_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, - size_t count); +static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count); static ssize_t _file_read(dm_item_t item, unsigned index, void *buf, size_t count); static int _file_clear(dm_item_t item); -static int _file_restart(dm_reset_reason reason); static int _file_initialize(unsigned max_offset); static void _file_shutdown(); /* Private Ram based Operations */ -static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, - size_t count); +static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_t count); static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count); static int _ram_clear(dm_item_t item); -static int _ram_restart(dm_reset_reason reason); static int _ram_initialize(unsigned max_offset); static void _ram_shutdown(); typedef struct dm_operations_t { - ssize_t (*write)(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count); + ssize_t (*write)(dm_item_t item, unsigned index, const void *buf, size_t count); ssize_t (*read)(dm_item_t item, unsigned index, void *buf, size_t count); int (*clear)(dm_item_t item); - int (*restart)(dm_reset_reason reason); int (*initialize)(unsigned max_offset); void (*shutdown)(); int (*wait)(px4_sem_t *sem); @@ -91,7 +86,6 @@ static constexpr dm_operations_t dm_file_operations = { .write = _file_write, .read = _file_read, .clear = _file_clear, - .restart = _file_restart, .initialize = _file_initialize, .shutdown = _file_shutdown, .wait = px4_sem_wait, @@ -101,7 +95,6 @@ static constexpr dm_operations_t dm_ram_operations = { .write = _ram_write, .read = _ram_read, .clear = _ram_clear, - .restart = _ram_restart, .initialize = _ram_initialize, .shutdown = _ram_shutdown, .wait = px4_sem_wait, @@ -127,7 +120,6 @@ typedef enum { dm_write_func = 0, dm_read_func, dm_clear_func, - dm_restart_func, dm_number_of_funcs } dm_function_t; @@ -142,7 +134,6 @@ typedef struct { struct { dm_item_t item; unsigned index; - dm_persitence_t persistence; const void *buf; size_t count; } write_params; @@ -155,9 +146,6 @@ typedef struct { struct { dm_item_t item; } clear_params; - struct { - dm_reset_reason reason; - } restart_params; }; } work_q_item_t; @@ -172,7 +160,6 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_WAYPOINTS_ONBOARD_MAX, DM_KEY_MISSION_STATE_MAX, DM_KEY_COMPAT_MAX }; @@ -185,7 +172,6 @@ static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE, sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, sizeof(struct mission_s) + DM_SECTOR_HDR_SIZE, sizeof(struct dataman_compat_s) + DM_SECTOR_HDR_SIZE }; @@ -393,7 +379,7 @@ calculate_offset(dm_item_t item, unsigned index) /* Each data item is stored as follows * * byte 0: Length of user data item - * byte 1: Persistence of this data item + * byte 1: Unused (previously persistence of this data item) * byte 2: Unused (for future use) * byte 3: Unused (for future use) * byte DM_SECTOR_HDR_SIZE... : data item value @@ -402,10 +388,8 @@ calculate_offset(dm_item_t item, unsigned index) */ /* write to the data manager RAM buffer */ -static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, - size_t count) +static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_t count) { - /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -425,9 +409,9 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persis return -1; } - /* Write out the data, prefixed with length and persistence level */ + /* Write out the data, prefixed with length */ buffer[0] = count; - buffer[1] = persistence; + buffer[1] = 0; buffer[2] = 0; buffer[3] = 0; @@ -441,7 +425,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persis /* write to the data manager file */ static ssize_t -_file_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count) +_file_write(dm_item_t item, unsigned index, const void *buf, size_t count) { unsigned char buffer[g_per_item_size[item]]; @@ -458,9 +442,9 @@ _file_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const v return -E2BIG; } - /* Write out the data, prefixed with length and persistence level */ + /* Write out the data, prefixed with length */ buffer[0] = count; - buffer[1] = persistence; + buffer[1] = 0; buffer[2] = 0; buffer[3] = 0; @@ -707,118 +691,6 @@ _file_clear(dm_item_t item) return result; } -/* Tell the data manager about the type of the last reset */ -static int _ram_restart(dm_reset_reason reason) -{ - uint8_t *buffer = dm_operations_data.ram.data; - - /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ - - /* Loop through all of the data segments and delete those that are not persistent */ - - for (int item = (int)DM_KEY_SAFE_POINTS; item < (int)DM_KEY_NUM_KEYS; item++) { - for (unsigned i = 0; i < g_per_item_max_index[item]; i++) { - /* check if segment contains data */ - if (buffer[0]) { - bool clear_entry = false; - - /* Whether data gets deleted depends on reset type and data segment's persistence setting */ - if (reason == DM_INIT_REASON_POWER_ON) { - if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { - clear_entry = true; - } - - } else { - if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { - clear_entry = true; - } - } - - /* Set segment to unused if data does not persist */ - if (clear_entry) { - buffer[0] = 0; - } - } - - buffer += g_per_item_size[item]; - } - } - - return 0; -} - -static int -_file_restart(dm_reset_reason reason) -{ - int offset = 0; - int result = 0; - /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ - - /* Loop through all of the data segments and delete those that are not persistent */ - for (int item = (int)DM_KEY_SAFE_POINTS; item < (int)DM_KEY_NUM_KEYS; item++) { - for (unsigned i = 0; i < g_per_item_max_index[item]; i++) { - /* Get data segment at current offset */ - if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { - result = -1; - item = DM_KEY_NUM_KEYS; - break; - } - - uint8_t buffer[2]; - ssize_t len = read(dm_operations_data.file.fd, buffer, sizeof(buffer)); - - if (len != sizeof(buffer)) { - result = -1; - item = DM_KEY_NUM_KEYS; - break; - } - - /* check if segment contains data */ - if (buffer[0]) { - bool clear_entry = false; - - /* Whether data gets deleted depends on reset type and data segment's persistence setting */ - if (reason == DM_INIT_REASON_POWER_ON) { - if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { - clear_entry = true; - } - - } else { - if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { - clear_entry = true; - } - } - - /* Set segment to unused if data does not persist */ - if (clear_entry) { - if (lseek(dm_operations_data.file.fd, offset, SEEK_SET) != offset) { - result = -1; - item = DM_KEY_NUM_KEYS; - break; - } - - buffer[0] = 0; - - len = write(dm_operations_data.file.fd, buffer, 1); - - if (len != 1) { - result = -1; - item = DM_KEY_NUM_KEYS; - break; - } - } - } - - offset += g_per_item_size[item]; - } - } - - fsync(dm_operations_data.file.fd); - - /* tell the caller how it went */ - return result; -} - static int _file_initialize(unsigned max_offset) { @@ -864,7 +736,7 @@ _file_initialize(unsigned max_offset) /* Write current compat info */ struct dataman_compat_s compat_state; compat_state.key = DM_COMPAT_KEY; - int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, DM_PERSIST_POWER_ON_RESET, &compat_state, sizeof(compat_state)); + int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); if (ret != sizeof(compat_state)) { PX4_ERR("Failed writing compat: %d", ret); @@ -911,7 +783,7 @@ _ram_shutdown() /** Write to the data manager file */ __EXPORT ssize_t -dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count) +dm_write(dm_item_t item, unsigned index, const void *buf, size_t count) { work_q_item_t *work; @@ -932,7 +804,6 @@ dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void work->func = dm_write_func; work->write_params.item = item; work->write_params.index = index; - work->write_params.persistence = persistence; work->write_params.buf = buf; work->write_params.count = count; @@ -1060,30 +931,6 @@ dm_unlock(dm_item_t item) } } -/** Tell the data manager about the type of the last reset */ -__EXPORT int -dm_restart(dm_reset_reason reason) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - /* get a work item and queue up a restart request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_restart create_work_item failed"); - return -1; - } - - work->func = dm_restart_func; - work->restart_params.reason = reason; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - return enqueue_work_item_and_wait_for_result(work); -} - static int task_main(int argc, char *argv[]) { @@ -1184,9 +1031,7 @@ task_main(int argc, char *argv[]) case dm_write_func: g_func_counts[dm_write_func]++; work->result = - g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.persistence, - work->write_params.buf, - work->write_params.count); + g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.buf, work->write_params.count); break; case dm_read_func: @@ -1200,11 +1045,6 @@ task_main(int argc, char *argv[]) work->result = g_dm_ops->clear(work->clear_params.item); break; - case dm_restart_func: - g_func_counts[dm_restart_func]++; - work->result = g_dm_ops->restart(work->restart_params.reason); - break; - default: /* should never happen */ work->result = -1; break; @@ -1284,7 +1124,6 @@ status() PX4_INFO("Writes %u", g_func_counts[dm_write_func]); PX4_INFO("Reads %u", g_func_counts[dm_read_func]); PX4_INFO("Clears %u", g_func_counts[dm_clear_func]); - PX4_INFO("Restarts %u", g_func_counts[dm_restart_func]); PX4_INFO("Max Q lengths work %u, free %u", g_work_q.max_size, g_free_q.max_size); perf_print_counter(_dm_read_perf); perf_print_counter(_dm_write_perf); @@ -1328,9 +1167,6 @@ check for geofence violations. PRINT_MODULE_USAGE_PARAM_STRING('f', nullptr, "", "Storage file", true); PRINT_MODULE_USAGE_PARAM_FLAG('r', "Use RAM backend (NOT persistent)", true); PRINT_MODULE_USAGE_PARAM_COMMENT("The options -f and -r are mutually exclusive. If nothing is specified, a file 'dataman' is used"); - - PRINT_MODULE_USAGE_COMMAND_DESCR("poweronrestart", "Restart dataman (on power on)"); - PRINT_MODULE_USAGE_COMMAND_DESCR("inflightrestart", "Restart dataman (in flight)"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -1425,12 +1261,6 @@ dataman_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { status(); - } else if (!strcmp(argv[1], "poweronrestart")) { - dm_restart(DM_INIT_REASON_POWER_ON); - - } else if (!strcmp(argv[1], "inflightrestart")) { - dm_restart(DM_INIT_REASON_IN_FLIGHT); - } else { usage(); return -1; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index c7bb0b0c90..bcc5b2984a 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -52,7 +52,6 @@ typedef enum { DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ - DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_COMPAT, DM_KEY_NUM_KEYS /* Total number of item types defined */ @@ -64,7 +63,6 @@ enum { DM_KEY_FENCE_POINTS_MAX = 16, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = (NUM_MISSIONS_SUPPORTED / 10), DM_KEY_MISSION_STATE_MAX = 1, DM_KEY_COMPAT_MAX = 1 }; @@ -75,24 +73,10 @@ enum { DM_KEY_FENCE_POINTS_MAX = 64, DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, - DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_MISSION_STATE_MAX = 1, DM_KEY_COMPAT_MAX = 1 }; #endif -/** Data persistence levels */ -typedef enum { - DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ - DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ - DM_PERSIST_VOLATILE /* Data does not survive resets */ -} dm_persitence_t; - -/** The reason for the last reset */ -typedef enum { - DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ - DM_INIT_REASON_VOLATILE /* Data does not survive reset */ -} dm_reset_reason; struct dataman_compat_s { uint64_t key; @@ -120,7 +104,6 @@ __EXPORT ssize_t dm_write( dm_item_t item, /* The item type to store */ unsigned index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ const void *buffer, /* Pointer to caller data buffer */ size_t buflen /* Length in bytes of data to retrieve */ ); @@ -157,12 +140,6 @@ dm_clear( dm_item_t item /* The item type to clear */ ); -/** Tell the data manager about the type of the last reset */ -__EXPORT int -dm_restart( - dm_reset_reason restart_type /* The last reset type */ -); - #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b4b2ab0b73..220e283b0e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -166,7 +166,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun PX4_ERR("DM_KEY_MISSION_STATE lock failed"); } - int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); /* unlock MISSION_STATE item */ if (dm_lock_ret == 0) { @@ -197,6 +197,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun return PX4_ERROR; } } + int MavlinkMissionManager::update_geofence_count(unsigned count) { @@ -205,7 +206,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data /* update stats in dataman */ - int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); if (res == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_FENCE] = count; @@ -232,7 +233,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) stats.update_counter = ++_safepoint_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_SAFE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); if (res == sizeof(mission_stats_entry_s)) { _count[MAV_MISSION_TYPE_RALLY] = count; @@ -1122,8 +1123,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { dm_item_t dm_item = _transfer_dataman_id; - write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, - sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); if (!write_failed) { /* waypoint marked as current */ @@ -1159,7 +1159,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.frame = mission_item.frame; if (!check_failed) { - write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); } @@ -1172,7 +1172,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_safe_point.lon = mission_item.lon; mission_safe_point.alt = mission_item.altitude; mission_safe_point.frame = mission_item.frame; - write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_safe_point, + write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); } break; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp index e6605b525d..5811863889 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp @@ -54,7 +54,6 @@ extern "C" { dm_write( dm_item_t item, /* The item type to store */ unsigned index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ const void *buffer, /* Pointer to caller data buffer */ size_t buflen /* Length in bytes of data to retrieve */ ) {return 0;}; @@ -90,11 +89,5 @@ extern "C" { dm_clear( dm_item_t item /* The item type to clear */ ) {return 0;}; - - /** Tell the data manager about the type of the last reset */ - __EXPORT int - dm_restart( - dm_reset_reason restart_type /* The last reset type */ - ) {return 0;}; } diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index ac78ff94b5..9b92b3d0dd 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -537,8 +537,7 @@ Geofence::loadFromFile(const char *filename) } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, DM_PERSIST_POWER_ON_RESET, &vertex, - sizeof(vertex)) != sizeof(vertex)) { + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, &vertex, sizeof(vertex)) != sizeof(vertex)) { goto error; } @@ -571,14 +570,13 @@ Geofence::loadFromFile(const char *filename) if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == sizeof(mission_fence_point_s)) { mission_fence_point.vertex_count = pointCounter; - dm_write(DM_KEY_FENCE_POINTS, seq, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, - sizeof(mission_fence_point_s)); + dm_write(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)); } } mission_stats_entry_s stats; stats.num_items = pointCounter; - rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s)); + rc = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 814ed2cfc6..84c6d0a2a1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,6 +67,30 @@ Mission::Mission(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + mission_init(); +} + +void Mission::mission_init() +{ + // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start + mission_s mission{}; + + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if ((mission.timestamp != 0) + && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { + if (mission.count > 0) { + PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count); + } + + } else { + PX4_ERR("reading mission state failed"); + + // initialize mission state in dataman + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.timestamp = hrt_absolute_time(); + dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + } + } } void @@ -1578,7 +1602,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, @@ -1645,8 +1669,7 @@ Mission::save_mission_state() mission_state.current_seq = _current_mission_index; mission_state.timestamp = hrt_absolute_time(); - if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, - sizeof(mission_s)) != sizeof(mission_s)) { + if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { PX4_ERR("Can't save mission state"); } @@ -1667,8 +1690,7 @@ Mission::save_mission_state() events::send(events::ID("mission_invalid_mission_state"), events::Log::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)) { + if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { PX4_ERR("Can't save mission state"); } @@ -1765,7 +1787,7 @@ Mission::reset_mission(struct mission_s &mission) if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; - if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET, &item, len) != len) { + if (dm_write(dm_current, index, &item, len) != len) { PX4_WARN("could not save mission item during reset"); break; } @@ -1784,7 +1806,7 @@ Mission::reset_mission(struct mission_s &mission) mission.current_seq = 0; } - dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); } dm_unlock(DM_KEY_MISSION_STATE); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e8acc98bf7..24dd8bf1c9 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -105,6 +105,8 @@ public: void set_execution_mode(const uint8_t mode); private: + void mission_init(); + /** * Update mission topic */ diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index b8af542a3e..b32a9569b4 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -85,7 +85,8 @@ task_main(int argc, char *argv[]) } srand(hrt_absolute_time() ^ my_id); - unsigned hit = 0, miss = 0; + unsigned hit = 0; + unsigned miss = 0; hrt_abstime wstart = hrt_absolute_time(); for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { @@ -94,11 +95,11 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, len); //PX4_INFO("ret: %d", ret); if (ret != len) { - PX4_WARN("task %d: write failed (%d), index %d, length %d", my_id, ret, hash, len); + PX4_WARN("task %d: write failed ret=%d, index: %d, length: %d", my_id, ret, hash, len); goto fail; } @@ -114,11 +115,11 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { unsigned hash = i ^ my_id; - ssize_t len2; + ssize_t len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer)); ssize_t len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { - PX4_WARN("task %d: read failed length test, index %d", my_id, hash); + if (len2 != len) { + PX4_WARN("task %d: read failed length test, index %d, ret=%zu, len=%zu", my_id, hash, len2, len); goto fail; } @@ -208,28 +209,11 @@ int test_dataman(int argc, char *argv[]) return -1; } - dm_restart(DM_INIT_REASON_IN_FLIGHT); - for (i = 0; i < NUM_MISSIONS_TEST; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { break; } } - if (i >= NUM_MISSIONS_TEST) { - PX4_ERR("Restart in-flight failed"); - return -1; - - } - - dm_restart(DM_INIT_REASON_POWER_ON); - - for (i = 0; i < NUM_MISSIONS_TEST; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { - PX4_ERR("Restart power-on failed"); - return -1; - } - } - return 0; }