mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 15:40:31 +08:00
dataman: remove obsolete persistence and reset reason
This commit is contained in:
Daniel Agar
GitHub
@@ -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) {
|
||||
|
||||
@@ -72,7 +72,6 @@
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/iridiumsbd_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
@@ -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<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
|
||||
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
|
||||
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
|
||||
|
||||
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
|
||||
|
||||
|
||||
+13
-183
@@ -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, "<file>", "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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;};
|
||||
}
|
||||
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -105,6 +105,8 @@ public:
|
||||
void set_execution_mode(const uint8_t mode);
|
||||
private:
|
||||
|
||||
void mission_init();
|
||||
|
||||
/**
|
||||
* Update mission topic
|
||||
*/
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user