aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/modules/dataman/dataman.c54
-rw-r--r--src/modules/dataman/dataman.h26
-rw-r--r--src/modules/mavlink/mavlink_commands.cpp35
-rw-r--r--src/modules/mavlink/mavlink_commands.h2
-rw-r--r--src/modules/mavlink/mavlink_main.cpp449
-rw-r--r--src/modules/mavlink/mavlink_main.h30
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp964
-rw-r--r--src/modules/mavlink/mavlink_messages.h15
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.cpp49
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.h30
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp1
-rw-r--r--src/modules/mavlink/mavlink_stream.h22
-rw-r--r--src/modules/mavlink/waypoints.h2
-rw-r--r--src/modules/navigator/navigator_main.cpp7
-rw-r--r--src/modules/uORB/topics/mission.h4
15 files changed, 1047 insertions, 643 deletions
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 1a65313e8..4a89c7d41 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -61,6 +61,8 @@ __EXPORT int dataman_main(int argc, char *argv[]);
__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
__EXPORT int dm_clear(dm_item_t item);
+__EXPORT void dm_lock(dm_item_t item);
+__EXPORT void dm_unlock(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type);
/** Types of function calls supported by the worker task */
@@ -113,12 +115,17 @@ 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_WAYPOINTS_ONBOARD_MAX,
+ DM_KEY_MISSION_STATE_MAX
};
/* Table of offset for index 0 of each item type */
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+/* Item type lock mutexes */
+static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static sem_t g_sys_state_mutex;
+
/* The data manager store file handle and file name */
static int g_fd = -1, g_task_fd = -1;
static const char *k_data_manager_device_path = "/fs/microsd/dataman";
@@ -138,22 +145,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread
sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */
sem_t g_init_sema;
-static bool g_task_should_exit; /**< if true, dataman task should exit */
+static bool g_task_should_exit; /**< if true, dataman task should exit */
#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
static void init_q(work_q_t *q)
{
- sq_init(&(q->q)); /* Initialize the NuttX queue structure */
+ sq_init(&(q->q)); /* Initialize the NuttX queue structure */
sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */
- q->size = q->max_size = 0; /* Queue is initially empty */
+ q->size = q->max_size = 0; /* Queue is initially empty */
}
static inline void
destroy_q(work_q_t *q)
{
- sem_destroy(&(q->mutex)); /* Destroy the queue lock */
+ sem_destroy(&(q->mutex)); /* Destroy the queue lock */
}
static inline void
@@ -317,7 +324,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
buffer[1] = persistence;
buffer[2] = 0;
buffer[3] = 0;
- memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+ if (count > 0) {
+ memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+ }
count += DM_SECTOR_HDR_SIZE;
len = -1;
@@ -567,6 +576,32 @@ dm_clear(dm_item_t item)
return enqueue_work_item_and_wait_for_result(work);
}
+__EXPORT void
+dm_lock(dm_item_t item)
+{
+ /* Make sure data manager has been started and is not shutting down */
+ if ((g_fd < 0) || g_task_should_exit)
+ return;
+ if (item >= DM_KEY_NUM_KEYS)
+ return;
+ if (g_item_locks[item]) {
+ sem_wait(g_item_locks[item]);
+ }
+}
+
+__EXPORT void
+dm_unlock(dm_item_t item)
+{
+ /* Make sure data manager has been started and is not shutting down */
+ if ((g_fd < 0) || g_task_should_exit)
+ return;
+ if (item >= DM_KEY_NUM_KEYS)
+ return;
+ if (g_item_locks[item]) {
+ sem_post(g_item_locks[item]);
+ }
+}
+
/* Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(dm_reset_reason reason)
@@ -607,6 +642,12 @@ task_main(int argc, char *argv[])
for (unsigned i = 0; i < dm_number_of_funcs; i++)
g_func_counts[i] = 0;
+ /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
+ sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+ for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++)
+ g_item_locks[i] = NULL;
+ g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
+
g_task_should_exit = false;
init_q(&g_work_q);
@@ -742,6 +783,7 @@ task_main(int argc, char *argv[])
destroy_q(&g_work_q);
destroy_q(&g_free_q);
sem_destroy(&g_work_queued_sema);
+ sem_destroy(&g_sys_state_mutex);
return 0;
}
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 1dfb26f73..215ec4c76 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -53,6 +53,7 @@ extern "C" {
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
+ DM_KEY_MISSION_STATE, /* Persistent mission state */
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
@@ -62,7 +63,8 @@ extern "C" {
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
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_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+ DM_KEY_MISSION_STATE_MAX = 1
};
/** Data persistence levels */
@@ -101,6 +103,18 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
+ /** Lock all items of this type */
+ __EXPORT void
+ dm_lock(
+ dm_item_t item /* The item type to clear */
+ );
+
+ /** Unlock all items of this type */
+ __EXPORT void
+ dm_unlock(
+ dm_item_t item /* The item type to clear */
+ );
+
/** Erase all items of this type */
__EXPORT int
dm_clear(
@@ -113,6 +127,16 @@ extern "C" {
dm_reset_reason restart_type /* The last reset type */
);
+ /* NOTE: The following structure defines the persistent system state data stored in the single
+ entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that
+ needs to survive restarts. This definition is application specific so it doesn't really belong
+ in this header, but till I find it a better home here it is */
+
+ typedef struct {
+ char offboard_waypoint_id; /* the index of the active offboard waypoint data */
+ /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */
+ } persistent_system_state_t;
+
#ifdef __cplusplus
}
#endif
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
index 1c1e097a4..fccd4d9a5 100644
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -40,34 +40,31 @@
#include "mavlink_commands.h"
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
- _cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
-}
-
-MavlinkCommandsStream::~MavlinkCommandsStream()
-{
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
- if (_cmd_sub->update(t)) {
+ struct vehicle_command_s cmd;
+
+ if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
- if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
+ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
- _cmd->target_system,
- _cmd->target_component,
- _cmd->command,
- _cmd->confirmation,
- _cmd->param1,
- _cmd->param2,
- _cmd->param3,
- _cmd->param4,
- _cmd->param5,
- _cmd->param6,
- _cmd->param7);
+ cmd.target_system,
+ cmd.target_component,
+ cmd.command,
+ cmd.confirmation,
+ cmd.param1,
+ cmd.param2,
+ cmd.param3,
+ cmd.param4,
+ cmd.param5,
+ cmd.param6,
+ cmd.param7);
}
}
}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
index 6255d65df..abdba34b9 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -55,10 +55,10 @@ private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
+ uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
- ~MavlinkCommandsStream();
void update(const hrt_abstime t);
};
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3a656ae4d..5c6ef68ee 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -236,6 +236,12 @@ Mavlink::Mavlink() :
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_message_buffer({}),
+ _param_initialized(false),
+ _param_system_id(0),
+ _param_component_id(0),
+ _param_system_type(0),
+ _param_use_hil_gps(0),
+
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
@@ -494,44 +500,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
void Mavlink::mavlink_update_system(void)
{
- static bool initialized = false;
- static param_t param_system_id;
- static param_t param_component_id;
- static param_t param_system_type;
- static param_t param_use_hil_gps;
- if (!initialized) {
- param_system_id = param_find("MAV_SYS_ID");
- param_component_id = param_find("MAV_COMP_ID");
- param_system_type = param_find("MAV_TYPE");
- param_use_hil_gps = param_find("MAV_USEHILGPS");
- initialized = true;
+ if (!_param_initialized) {
+ _param_system_id = param_find("MAV_SYS_ID");
+ _param_component_id = param_find("MAV_COMP_ID");
+ _param_system_type = param_find("MAV_TYPE");
+ _param_use_hil_gps = param_find("MAV_USEHILGPS");
+ _param_initialized = true;
}
/* update system and component id */
int32_t system_id;
- param_get(param_system_id, &system_id);
+ param_get(_param_system_id, &system_id);
if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id;
}
int32_t component_id;
- param_get(param_component_id, &component_id);
+ param_get(_param_component_id, &component_id);
if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id;
}
int32_t system_type;
- param_get(param_system_type, &system_type);
+ param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
- param_get(param_use_hil_gps, &use_hil_gps);
+ param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
@@ -819,7 +820,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
- sprintf(buf, "[mavlink pm] unknown: %s", name);
+ sprintf(buf, "[pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
} else {
@@ -963,16 +964,56 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
{
+ persistent_system_state_t sys_state;
+
+ /* init WPM state */
state->size = 0;
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
+ state->dataman_id = 0;
state->current_state = MAVLINK_WPM_STATE_IDLE;
state->current_partner_sysid = 0;
state->current_partner_compid = 0;
+ state->current_dataman_id = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timestamp_last_send_request = 0;
- state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
- state->current_dataman_id = 0;
+ state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
+ state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT;
+
+ int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state));
+ if (sys_state_size == sizeof(sys_state)) {
+ if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) {
+ state->dataman_id = sys_state.offboard_waypoint_id;
+
+ if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); }
+
+ /* count waypoints in current waypoints storage */
+ dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
+ struct mission_item_s mission_item;
+
+ int seq = 0;
+ while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
+ seq++;
+ }
+
+ if (_verbose) { warnx("WPM init: found %d items", seq); }
+
+ state->size = seq;
+
+ } else {
+ if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); }
+ }
+
+ } else {
+ if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); }
+ }
+
+ /* init mission topic */
+ mission.count = state->size;
+ mission.dataman_id = state->dataman_id;
+ mission.current_index = -1; // TODO store current index in dataman?
+
+ publish_mission();
}
/*
@@ -990,7 +1031,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
- if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
+ if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
/*
@@ -1014,13 +1055,12 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
mavlink_missionlib_send_message(&msg);
} else if (seq == 0 && _wpm->size == 0) {
-
/* don't broadcast if no WPs */
} else {
- mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
+ if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); }
- if (_verbose) { warnx("ERROR: index out of bounds"); }
+ mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
}
}
@@ -1036,25 +1076,15 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
- if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
+ if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
-
+ dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
struct mission_item_s mission_item;
- ssize_t len = sizeof(struct mission_item_s);
-
- dm_item_t dm_current;
- if (_wpm->current_dataman_id == 0) {
- dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
- } else {
- dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
- }
-
- if (dm_read(dm_current, seq, &mission_item, len) == len) {
+ if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
/* create mission_item_s from mavlink_mission_item_t */
mavlink_mission_item_t wp;
@@ -1067,13 +1097,13 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp);
mavlink_missionlib_send_message(&msg);
- if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
+ if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); }
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
- if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
+ if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); }
}
}
@@ -1089,12 +1119,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
mavlink_missionlib_send_message(&msg);
_wpm->timestamp_last_send_request = hrt_absolute_time();
- if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
+ if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
- if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
+ if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); }
}
}
@@ -1115,23 +1145,23 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
- if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
+ if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); }
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
{
/* check for timed-out operations */
- if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
+ if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("Operation timeout");
- if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
+ if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
- } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+ } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* try to get WP again after short timeout */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
@@ -1148,23 +1178,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
- if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
- _wpm->timestamp_lastaction = now;
+ if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+ if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) {
+ _wpm->timestamp_lastaction = now;
- if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
- if (_wpm->current_wp_id == _wpm->size - 1) {
+ if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
+ if (_wpm->current_wp_id == _wpm->size - 1) {
+ if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); }
- _wpm->current_state = MAVLINK_WPM_STATE_IDLE;
- _wpm->current_wp_id = 0;
+ _wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+ _wpm->current_wp_id = 0;
+ }
}
- }
- } else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
+ } else {
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch");
- if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
+ if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
+ }
}
-
break;
}
@@ -1177,6 +1209,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.seq < _wpm->size) {
+ if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); }
mission.current_index = wpc.seq;
publish_mission();
@@ -1185,16 +1218,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
- mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
+ if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); }
- if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
+ mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
}
} else {
- mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
- if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
+ if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
+ mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
}
}
@@ -1210,23 +1242,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (_wpm->size > 0) {
-
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
_wpm->current_partner_compid = msg->compid;
} else {
- if (_verbose) { warnx("No waypoints send"); }
+ if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); }
}
_wpm->current_count = _wpm->size;
mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count);
} else {
- mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
+ if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
- if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
+ mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
}
}
@@ -1237,88 +1268,73 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
- if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
- _wpm->timestamp_lastaction = now;
-
- if (wpr.seq >= _wpm->size) {
-
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-
- if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
-
- break;
- }
-
- /*
- * Ensure that we are in the correct state and that the first request has id 0
- * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
- */
- if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
+ if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+ if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) {
+ _wpm->timestamp_lastaction = now;
- if (wpr.seq == 0) {
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+ if (wpr.seq >= _wpm->size) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); }
- _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
+ break;
+ }
- } else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
+ /*
+ * Ensure that we are in the correct state and that the first request has id 0
+ * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
+ */
+ if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
+ if (wpr.seq == 0) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
- if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
+ _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
- break;
- }
+ } else {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); }
- } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
+ break;
+ }
- if (wpr.seq == _wpm->current_wp_id) {
+ } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
+ if (wpr.seq == _wpm->current_wp_id) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+ } else if (wpr.seq == _wpm->current_wp_id + 1) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
- } else if (wpr.seq == _wpm->current_wp_id + 1) {
+ } else {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
+ break;
+ }
} else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-
- if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); }
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
break;
}
- } else {
-
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
- if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
-
- break;
- }
+ _wpm->current_wp_id = wpr.seq;
+ _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
- _wpm->current_wp_id = wpr.seq;
- _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+ if (wpr.seq < _wpm->size) {
+ mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
- if (wpr.seq < _wpm->size) {
+ } else {
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); }
- mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
+ mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+ }
} else {
- mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch");
- if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
- }
-
-
- } else {
- //we we're target but already communicating with someone else
- if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) {
-
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
- if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
+ if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
}
}
-
break;
}
@@ -1332,48 +1348,51 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
- if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
+ if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
- mavlink_missionlib_send_gcs_string("COUNT 0");
-
- if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+ if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+ mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION");
break;
}
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
-
- _wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
- _wpm->current_wp_id = 0;
- _wpm->current_partner_sysid = msg->sysid;
- _wpm->current_partner_compid = msg->compid;
- _wpm->current_count = wpc.count;
-
- mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
+ if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (_wpm->current_wp_id == 0) {
- mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
+ /* looks like our WAYPOINT_REQUEST was lost, try again */
+ if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); }
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
+ mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
} else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+ if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); }
- if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
+ mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
+ break;
}
} else {
- mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
+ if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); }
- if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
+ mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
+ break;
}
+
+ _wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
+ _wpm->current_wp_id = 0;
+ _wpm->current_partner_sysid = msg->sysid;
+ _wpm->current_partner_compid = msg->compid;
+ _wpm->current_count = wpc.count;
+ _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission
+
+ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
break;
@@ -1383,32 +1402,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
-
_wpm->timestamp_lastaction = now;
- /*
- * ensure that we are in the correct state and that the first waypoint has id 0
+ /* Ensure that we are in the correct state and that the first waypoint has id 0
* and the following waypoints have the correct ids
*/
-
if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-
if (wp.seq != 0) {
- mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
- warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); }
break;
}
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-
if (wp.seq >= _wpm->current_count) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); }
+
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
- warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
break;
}
if (wp.seq != _wpm->current_wp_id) {
- warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); }
+
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
@@ -1417,60 +1432,92 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
struct mission_item_s mission_item;
-
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
if (ret != OK) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
+
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret);
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
- ssize_t len = sizeof(struct mission_item_s);
+ dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
- dm_item_t dm_next;
+ if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); }
- if (_wpm->current_dataman_id == 0) {
- dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
- mission.dataman_id = 1;
-
- } else {
- dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
- mission.dataman_id = 0;
- }
-
- if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
-// if (wp.current) {
-// warnx("current is: %d", wp.seq);
-// mission.current_index = wp.seq;
-// }
- // XXX ignore current set
- mission.current_index = -1;
+ if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
_wpm->current_wp_id = wp.seq + 1;
if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+ /* got all new mission items successfully */
+ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); }
- if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); }
+ _wpm->current_state = MAVLINK_WPM_STATE_IDLE;
- mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+ /* write terminator item */
+ if (_wpm->current_count < _wpm->max_size) {
+ if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); }
- mission.count = _wpm->current_count;
+ mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+ break;
+ }
+ }
- publish_mission();
+ /* offboard mission data saved correctly, now update the persistent system state */
+ persistent_system_state_t state;
+ bool dm_result;
- _wpm->current_dataman_id = mission.dataman_id;
- _wpm->size = _wpm->current_count;
+ /* since we are doing a read-modify-write we must lock the item type */
+ dm_lock(DM_KEY_MISSION_STATE);
- _wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+ /* first read in the current state data, there may eventually be data other than the offboard index
+ * and we must preserve it */
+ ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state));
+ if (dm_state_size != sizeof(state)) {
+ warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state));
+ }
+
+ /* set new dataman storage ID */
+ state.offboard_waypoint_id = _wpm->current_dataman_id;
+
+ /* write back to dataman */
+ dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state);
+
+ dm_unlock(DM_KEY_MISSION_STATE);
+
+ if (dm_result) {
+ if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); }
+
+ mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+ mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
+
+ } else {
+ mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+
+ /* update WPM state */
+ _wpm->dataman_id = _wpm->current_dataman_id;
+ _wpm->size = _wpm->current_count;
+
+ /* update mission topic */
+ mission.dataman_id = _wpm->dataman_id;
+ mission.count = _wpm->current_count;
+ mission.current_index = 0;
+
+ publish_mission();
+ }
} else {
+ /* request next item */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
@@ -1485,28 +1532,34 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
- _wpm->timestamp_lastaction = now;
+ /* clear only active dataman storage, don't change storage id */
+ dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
- _wpm->size = 0;
+ if (dm_clear(dm_id) == OK) {
+ if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
- /* prepare mission topic */
- mission.dataman_id = -1;
- mission.count = 0;
- mission.current_index = -1;
- publish_mission();
-
- if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+ /* update WPM state */
+ _wpm->timestamp_lastaction = now;
+ _wpm->size = 0;
+
+ /* update mission topic */
+ mission.count = 0;
+ mission.current_index = -1;
+
+ publish_mission();
+
} else {
+ if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); }
+
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
}
-
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
- if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
+ if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
}
}
@@ -1523,15 +1576,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
- uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
-
- uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
+ uint8_t buf[MAVLINK_MAX_PACKET_LEN];
- mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
+ uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+ mavlink_send_uart_bytes(_channel, buf, len);
}
-
int
Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
{
@@ -1610,6 +1661,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
if (interval > 0) {
/* search for stream with specified name in supported streams list */
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
/* create new instance */
stream = streams_list[i]->new_instance();
@@ -1915,7 +1967,7 @@ Mavlink::task_main(int argc, char *argv[])
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_passing_on) {
/* initialize message buffer if multiplexing is on */
- if (OK != message_buffer_init(500)) {
+ if (OK != message_buffer_init(300)) {
errx(1, "can't allocate message buffer, exiting");
}
@@ -1945,9 +1997,12 @@ Mavlink::task_main(int argc, char *argv[])
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+ uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+ uint64_t status_time = 0;
- struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+ struct vehicle_status_s status;
+ status_sub->update(&status_time, &status);
MavlinkCommandsStream commands_stream(this, _channel);
@@ -2004,14 +2059,14 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
- if (param_sub->update(t)) {
+ if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
- if (status_sub->update(t)) {
+ if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
- set_hil_enabled(status->hil_state == HIL_STATE_ON);
+ set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
/* update commands stream */
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 40edc4b85..61fd7afe2 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -78,24 +78,27 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_WPM_MAX_WP_COUNT 255
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
+#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
+#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
+#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
struct mavlink_wpm_storage {
- uint16_t size;
- uint16_t max_size;
- enum MAVLINK_WPM_STATES current_state;
- int16_t current_wp_id; ///< Waypoint in current transmission
- uint16_t current_count;
+ uint16_t size; ///< Count of waypoints in active mission
+ uint16_t max_size; ///< Maximal count of waypoints
+ int dataman_id; ///< Dataman storage ID for active mission
+ enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state
+ int16_t current_wp_id; ///< Waypoint ID in current transmission
+ uint16_t current_count; ///< Waypoints count in current transmission
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
+ int current_dataman_id; ///< Dataman storage ID for current transmission
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_last_send_request;
- uint32_t timeout;
- int current_dataman_id;
+ uint32_t action_timeout;
+ uint32_t retry_timeout;
};
@@ -277,11 +280,16 @@ private:
int size;
char *data;
};
- mavlink_message_buffer _message_buffer;
+ mavlink_message_buffer _message_buffer;
- pthread_mutex_t _message_buffer_mutex;
+ pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
+ bool _param_initialized;
+ param_t _param_system_id;
+ param_t _param_component_id;
+ param_t _param_system_type;
+ param_t _param_use_hil_gps;
/**
* Send one parameter.
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b58fb4cb8..f1cffb986 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -74,6 +74,8 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
+#include <systemlib/err.h>
+
#include "mavlink_messages.h"
@@ -214,50 +216,50 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
class MavlinkStreamHeartbeat : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamHeartbeat::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "HEARTBEAT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHeartbeat();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
-
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
-
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t)
{
- (void)status_sub->update(t);
- (void)pos_sp_triplet_sub->update(t);
-
- uint8_t mavlink_state = 0;
- uint8_t mavlink_base_mode = 0;
- uint32_t mavlink_custom_mode = 0;
- get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
- mavlink_msg_heartbeat_send(_channel,
- mavlink_system.type,
- MAV_AUTOPILOT_PX4,
- mavlink_base_mode,
- mavlink_custom_mode,
- mavlink_state);
-
+ struct vehicle_status_s status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
+
+ if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+ uint8_t mavlink_state = 0;
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+ mavlink_msg_heartbeat_send(_channel,
+ mavlink_system.type,
+ MAV_AUTOPILOT_PX4,
+ mavlink_base_mode,
+ mavlink_custom_mode,
+ mavlink_state);
+ }
}
};
@@ -265,44 +267,50 @@ protected:
class MavlinkStreamSysStatus : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamSysStatus::get_name_static();
+ }
+
+ static const char *get_name_static ()
{
return "SYS_STATUS";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamSysStatus();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
- status_sub->update(t);
- mavlink_msg_sys_status_send(_channel,
- status->onboard_control_sensors_present,
- status->onboard_control_sensors_enabled,
- status->onboard_control_sensors_health,
- status->load * 1000.0f,
- status->battery_voltage * 1000.0f,
- status->battery_current * 100.0f,
- status->battery_remaining * 100.0f,
- status->drop_rate_comm,
- status->errors_comm,
- status->errors_count1,
- status->errors_count2,
- status->errors_count3,
- status->errors_count4);
+ struct vehicle_status_s status;
+
+ if (status_sub->update(&status)) {
+ mavlink_msg_sys_status_send(_channel,
+ status.onboard_control_sensors_present,
+ status.onboard_control_sensors_enabled,
+ status.onboard_control_sensors_health,
+ status.load * 1000.0f,
+ status.battery_voltage * 1000.0f,
+ status.battery_current * 100.0f,
+ status.battery_remaining * 100.0f,
+ status.drop_rate_comm,
+ status.errors_comm,
+ status.errors_count1,
+ status.errors_count2,
+ status.errors_count3,
+ status.errors_count4);
+ }
}
};
@@ -310,23 +318,24 @@ protected:
class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
- MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
+ const char *get_name() const
{
+ return MavlinkStreamHighresIMU::get_name_static();
}
- const char *get_name()
+ static const char *get_name_static()
{
return "HIGHRES_IMU";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHighresIMU();
}
private:
MavlinkOrbSubscription *sensor_sub;
- struct sensor_combined_s *sensor;
+ uint64_t sensor_time;
uint64_t accel_timestamp;
uint64_t gyro_timestamp;
@@ -334,48 +343,57 @@ private:
uint64_t baro_timestamp;
protected:
+ explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+ sensor_time(0),
+ accel_timestamp(0),
+ gyro_timestamp(0),
+ mag_timestamp(0),
+ baro_timestamp(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
- sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (sensor_sub->update(t)) {
+ struct sensor_combined_s sensor;
+
+ if (sensor_sub->update(&sensor_time, &sensor)) {
uint16_t fields_updated = 0;
- if (accel_timestamp != sensor->accelerometer_timestamp) {
+ if (accel_timestamp != sensor.accelerometer_timestamp) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
- accel_timestamp = sensor->accelerometer_timestamp;
+ accel_timestamp = sensor.accelerometer_timestamp;
}
- if (gyro_timestamp != sensor->timestamp) {
+ if (gyro_timestamp != sensor.timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
- gyro_timestamp = sensor->timestamp;
+ gyro_timestamp = sensor.timestamp;
}
- if (mag_timestamp != sensor->magnetometer_timestamp) {
+ if (mag_timestamp != sensor.magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
- mag_timestamp = sensor->magnetometer_timestamp;
+ mag_timestamp = sensor.magnetometer_timestamp;
}
- if (baro_timestamp != sensor->baro_timestamp) {
+ if (baro_timestamp != sensor.baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
- baro_timestamp = sensor->baro_timestamp;
+ baro_timestamp = sensor.baro_timestamp;
}
mavlink_msg_highres_imu_send(_channel,
- sensor->timestamp,
- sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
- sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
- sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
- sensor->baro_pres_mbar, sensor->differential_pressure_pa,
- sensor->baro_alt_meter, sensor->baro_temp_celcius,
+ sensor.timestamp,
+ sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
+ sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
+ sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
+ sensor.baro_pres_mbar, sensor.differential_pressure_pa,
+ sensor.baro_alt_meter, sensor.baro_temp_celcius,
fields_updated);
}
}
@@ -385,34 +403,44 @@ protected:
class MavlinkStreamAttitude : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitude::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitude();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
protected:
+ explicit MavlinkStreamAttitude() : MavlinkStream(),
+ att_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sub->update(t)) {
+ struct vehicle_attitude_s att;
+
+ if (att_sub->update(&att_time, &att)) {
mavlink_msg_attitude_send(_channel,
- att->timestamp / 1000,
- att->roll, att->pitch, att->yaw,
- att->rollspeed, att->pitchspeed, att->yawspeed);
+ att.timestamp / 1000,
+ att.roll, att.pitch, att.yaw,
+ att.rollspeed, att.pitchspeed, att.yawspeed);
}
}
};
@@ -421,39 +449,49 @@ protected:
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitudeQuaternion::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE_QUATERNION";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeQuaternion();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
protected:
+ explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+ att_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sub->update(t)) {
+ struct vehicle_attitude_s att;
+
+ if (att_sub->update(&att_time, &att)) {
mavlink_msg_attitude_quaternion_send(_channel,
- att->timestamp / 1000,
- att->q[0],
- att->q[1],
- att->q[2],
- att->q[3],
- att->rollspeed,
- att->pitchspeed,
- att->yawspeed);
+ att.timestamp / 1000,
+ att.q[0],
+ att.q[1],
+ att.q[2],
+ att.q[3],
+ att.rollspeed,
+ att.pitchspeed,
+ att.yawspeed);
}
}
};
@@ -462,71 +500,82 @@ protected:
class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
- const char *get_name()
+
+ const char *get_name() const
+ {
+ return MavlinkStreamVFRHUD::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "VFR_HUD";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamVFRHUD();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
MavlinkOrbSubscription *pos_sub;
- struct vehicle_global_position_s *pos;
+ uint64_t pos_time;
MavlinkOrbSubscription *armed_sub;
- struct actuator_armed_s *armed;
+ uint64_t armed_time;
MavlinkOrbSubscription *act_sub;
- struct actuator_controls_s *act;
+ uint64_t act_time;
MavlinkOrbSubscription *airspeed_sub;
- struct airspeed_s *airspeed;
+ uint64_t airspeed_time;
protected:
+ explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+ att_time(0),
+ pos_time(0),
+ armed_time(0),
+ act_time(0),
+ airspeed_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
-
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
- armed = (struct actuator_armed_s *)armed_sub->get_data();
-
act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
- act = (struct actuator_controls_s *)act_sub->get_data();
-
airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
- airspeed = (struct airspeed_s *)airspeed_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = att_sub->update(t);
- updated |= pos_sub->update(t);
- updated |= armed_sub->update(t);
- updated |= act_sub->update(t);
- updated |= airspeed_sub->update(t);
+ struct vehicle_attitude_s att;
+ struct vehicle_global_position_s pos;
+ struct actuator_armed_s armed;
+ struct actuator_controls_s act;
+ struct airspeed_s airspeed;
+
+ bool updated = att_sub->update(&att_time, &att);
+ updated |= pos_sub->update(&pos_time, &pos);
+ updated |= armed_sub->update(&armed_time, &armed);
+ updated |= act_sub->update(&act_time, &act);
+ updated |= airspeed_sub->update(&airspeed_time, &airspeed);
if (updated) {
- float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
- uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
- float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
+ float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+ uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+ float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
- airspeed->true_airspeed_m_s,
+ airspeed.true_airspeed_m_s,
groundspeed,
heading,
throttle,
- pos->alt,
- -pos->vel_d);
+ pos.alt,
+ -pos.vel_d);
}
}
};
@@ -535,41 +584,51 @@ protected:
class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGPSRawInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GPS_RAW_INT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSRawInt();
}
private:
MavlinkOrbSubscription *gps_sub;
- struct vehicle_gps_position_s *gps;
+ uint64_t gps_time;
protected:
+ explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+ gps_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
- gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (gps_sub->update(t)) {
+ struct vehicle_gps_position_s gps;
+
+ if (gps_sub->update(&gps_time, &gps)) {
mavlink_msg_gps_raw_int_send(_channel,
- gps->timestamp_position,
- gps->fix_type,
- gps->lat,
- gps->lon,
- gps->alt,
- cm_uint16_from_m_float(gps->eph),
- cm_uint16_from_m_float(gps->epv),
- gps->vel_m_s * 100.0f,
- _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- gps->satellites_visible);
+ gps.timestamp_position,
+ gps.fix_type,
+ gps.lat,
+ gps.lon,
+ gps.alt,
+ cm_uint16_from_m_float(gps.eph),
+ cm_uint16_from_m_float(gps.epv),
+ gps.vel_m_s * 100.0f,
+ _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ gps.satellites_visible);
}
}
};
@@ -578,49 +637,59 @@ protected:
class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGlobalPositionInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GLOBAL_POSITION_INT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionInt();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_global_position_s *pos;
+ uint64_t pos_time;
MavlinkOrbSubscription *home_sub;
- struct home_position_s *home;
+ uint64_t home_time;
protected:
+ explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+ pos_time(0),
+ home_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = pos_sub->update(t);
- updated |= home_sub->update(t);
+ struct vehicle_global_position_s pos;
+ struct home_position_s home;
+
+ bool updated = pos_sub->update(&pos_time, &pos);
+ updated |= home_sub->update(&home_time, &home);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
- pos->timestamp / 1000,
- pos->lat * 1e7,
- pos->lon * 1e7,
- pos->alt * 1000.0f,
- (pos->alt - home->alt) * 1000.0f,
- pos->vel_n * 100.0f,
- pos->vel_e * 100.0f,
- pos->vel_d * 100.0f,
- _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+ pos.timestamp / 1000,
+ pos.lat * 1e7,
+ pos.lon * 1e7,
+ pos.alt * 1000.0f,
+ (pos.alt - home.alt) * 1000.0f,
+ pos.vel_n * 100.0f,
+ pos.vel_e * 100.0f,
+ pos.vel_d * 100.0f,
+ _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
};
@@ -629,38 +698,48 @@ protected:
class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamLocalPositionNED::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "LOCAL_POSITION_NED";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionNED();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_local_position_s *pos;
+ uint64_t pos_time;
protected:
+ explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+ pos_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
- pos = (struct vehicle_local_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sub->update(t)) {
+ struct vehicle_local_position_s pos;
+
+ if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_local_position_ned_send(_channel,
- pos->timestamp / 1000,
- pos->x,
- pos->y,
- pos->z,
- pos->vx,
- pos->vy,
- pos->vz);
+ pos.timestamp / 1000,
+ pos.x,
+ pos.y,
+ pos.z,
+ pos.vx,
+ pos.vy,
+ pos.vz);
}
}
};
@@ -670,38 +749,48 @@ protected:
class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamViconPositionEstimate::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "VICON_POSITION_ESTIMATE";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamViconPositionEstimate();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_vicon_position_s *pos;
+ uint64_t pos_time;
protected:
+ explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+ pos_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
- pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sub->update(t)) {
+ struct vehicle_vicon_position_s pos;
+
+ if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_vicon_position_estimate_send(_channel,
- pos->timestamp / 1000,
- pos->x,
- pos->y,
- pos->z,
- pos->roll,
- pos->pitch,
- pos->yaw);
+ pos.timestamp / 1000,
+ pos.x,
+ pos.y,
+ pos.z,
+ pos.roll,
+ pos.pitch,
+ pos.yaw);
}
}
};
@@ -710,70 +799,87 @@ protected:
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGPSGlobalOrigin::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GPS_GLOBAL_ORIGIN";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSGlobalOrigin();
}
private:
MavlinkOrbSubscription *home_sub;
- struct home_position_s *home;
protected:
void subscribe(Mavlink *mavlink)
{
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t)
{
-
/* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */
if (home_sub->is_published()) {
- home_sub->update(t);
+ struct home_position_s home;
- mavlink_msg_gps_global_origin_send(_channel,
- (int32_t)(home->lat * 1e7),
- (int32_t)(home->lon * 1e7),
- (int32_t)(home->alt) * 1000.0f);
+ if (home_sub->update(&home)) {
+ mavlink_msg_gps_global_origin_send(_channel,
+ (int32_t)(home.lat * 1e7),
+ (int32_t)(home.lon * 1e7),
+ (int32_t)(home.alt) * 1000.0f);
+ }
}
}
};
-
+template <int N>
class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public:
- MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+ const char *get_name() const
{
- sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+ return MavlinkStreamServoOutputRaw<N>::get_name_static();
}
- const char *get_name()
+ static const char *get_name_static()
{
- return _name;
+ switch (N) {
+ case 0:
+ return "SERVO_OUTPUT_RAW_0";
+
+ case 1:
+ return "SERVO_OUTPUT_RAW_1";
+
+ case 2:
+ return "SERVO_OUTPUT_RAW_2";
+
+ case 3:
+ return "SERVO_OUTPUT_RAW_3";
+ }
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
- return new MavlinkStreamServoOutputRaw(_n);
+ return new MavlinkStreamServoOutputRaw<N>();
}
private:
MavlinkOrbSubscription *act_sub;
- struct actuator_outputs_s *act;
-
- char _name[20];
- unsigned int _n;
+ uint64_t act_time;
protected:
+ explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+ act_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
orb_id_t act_topics[] = {
@@ -783,24 +889,25 @@ protected:
ORB_ID(actuator_outputs_3)
};
- act_sub = mavlink->add_orb_subscription(act_topics[_n]);
- act = (struct actuator_outputs_s *)act_sub->get_data();
+ act_sub = mavlink->add_orb_subscription(act_topics[N]);
}
void send(const hrt_abstime t)
{
- if (act_sub->update(t)) {
+ struct actuator_outputs_s act;
+
+ if (act_sub->update(&act_time, &act)) {
mavlink_msg_servo_output_raw_send(_channel,
- act->timestamp / 1000,
- _n,
- act->output[0],
- act->output[1],
- act->output[2],
- act->output[3],
- act->output[4],
- act->output[5],
- act->output[6],
- act->output[7]);
+ act.timestamp / 1000,
+ N,
+ act.output[0],
+ act.output[1],
+ act.output[2],
+ act.output[3],
+ act.output[4],
+ act.output[5],
+ act.output[6],
+ act.output[7]);
}
}
};
@@ -809,51 +916,61 @@ protected:
class MavlinkStreamHILControls : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamHILControls::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "HIL_CONTROLS";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHILControls();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
+ uint64_t status_time;
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
+ uint64_t pos_sp_triplet_time;
MavlinkOrbSubscription *act_sub;
- struct actuator_outputs_s *act;
+ uint64_t act_time;
protected:
+ explicit MavlinkStreamHILControls() : MavlinkStream(),
+ status_time(0),
+ pos_sp_triplet_time(0),
+ act_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
-
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
-
act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
- act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = act_sub->update(t);
- (void)pos_sp_triplet_sub->update(t);
- (void)status_sub->update(t);
+ struct vehicle_status_s status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
+ struct actuator_outputs_s act;
+
+ bool updated = act_sub->update(&act_time, &act);
+ updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
+ updated |= status_sub->update(&status_time, &status);
- if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
+ if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
- get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
@@ -882,7 +999,7 @@ protected:
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
- out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
@@ -912,12 +1029,12 @@ protected:
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
- out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
- out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
@@ -936,37 +1053,42 @@ protected:
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GLOBAL_POSITION_SETPOINT_INT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionSetpointInt();
}
private:
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t)
{
- /* always send this message, even if it has not been updated */
- pos_sp_triplet_sub->update(t);
- mavlink_msg_global_position_setpoint_int_send(_channel,
- MAV_FRAME_GLOBAL,
- (int32_t)(pos_sp_triplet->current.lat * 1e7),
- (int32_t)(pos_sp_triplet->current.lon * 1e7),
- (int32_t)(pos_sp_triplet->current.alt * 1000),
- (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
+ struct position_setpoint_triplet_s pos_sp_triplet;
+
+ if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+ mavlink_msg_global_position_setpoint_int_send(_channel,
+ MAV_FRAME_GLOBAL,
+ (int32_t)(pos_sp_triplet.current.lat * 1e7),
+ (int32_t)(pos_sp_triplet.current.lon * 1e7),
+ (int32_t)(pos_sp_triplet.current.alt * 1000),
+ (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+ }
}
};
@@ -974,36 +1096,46 @@ protected:
class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamLocalPositionSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "LOCAL_POSITION_SETPOINT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionSetpoint();
}
private:
MavlinkOrbSubscription *pos_sp_sub;
- struct vehicle_local_position_setpoint_s *pos_sp;
+ uint64_t pos_sp_time;
protected:
+ explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+ pos_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
- pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sp_sub->update(t)) {
+ struct vehicle_local_position_setpoint_s pos_sp;
+
+ if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
- pos_sp->x,
- pos_sp->y,
- pos_sp->z,
- pos_sp->yaw);
+ pos_sp.x,
+ pos_sp.y,
+ pos_sp.z,
+ pos_sp.yaw);
}
}
};
@@ -1012,36 +1144,46 @@ protected:
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ROLL_PITCH_YAW_THRUST_SETPOINT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawThrustSetpoint();
}
private:
MavlinkOrbSubscription *att_sp_sub;
- struct vehicle_attitude_setpoint_s *att_sp;
+ uint64_t att_sp_time;
protected:
+ explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+ att_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
- att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sp_sub->update(t)) {
+ struct vehicle_attitude_setpoint_s att_sp;
+
+ if (att_sp_sub->update(&att_sp_time, &att_sp)) {
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
- att_sp->timestamp / 1000,
- att_sp->roll_body,
- att_sp->pitch_body,
- att_sp->yaw_body,
- att_sp->thrust);
+ att_sp.timestamp / 1000,
+ att_sp.roll_body,
+ att_sp.pitch_body,
+ att_sp.yaw_body,
+ att_sp.thrust);
}
}
};
@@ -1050,36 +1192,46 @@ protected:
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
}
private:
MavlinkOrbSubscription *att_rates_sp_sub;
- struct vehicle_rates_setpoint_s *att_rates_sp;
+ uint64_t att_rates_sp_time;
protected:
+ explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+ att_rates_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
- att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_rates_sp_sub->update(t)) {
+ struct vehicle_rates_setpoint_s att_rates_sp;
+
+ if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
- att_rates_sp->timestamp / 1000,
- att_rates_sp->roll,
- att_rates_sp->pitch,
- att_rates_sp->yaw,
- att_rates_sp->thrust);
+ att_rates_sp.timestamp / 1000,
+ att_rates_sp.roll,
+ att_rates_sp.pitch,
+ att_rates_sp.yaw,
+ att_rates_sp.thrust);
}
}
};
@@ -1088,47 +1240,82 @@ protected:
class MavlinkStreamRCChannelsRaw : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRCChannelsRaw::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "RC_CHANNELS_RAW";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRCChannelsRaw();
}
private:
MavlinkOrbSubscription *rc_sub;
- struct rc_input_values *rc;
+ uint64_t rc_time;
protected:
+ explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+ rc_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
- rc = (struct rc_input_values *)rc_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (rc_sub->update(t)) {
+ struct rc_input_values rc;
+
+ if (rc_sub->update(&rc_time, &rc)) {
const unsigned port_width = 8;
- for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+ // Deprecated message (but still needed for compatibility!)
+ for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
- rc->timestamp_publication / 1000,
+ rc.timestamp_publication / 1000,
i,
- (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
- rc->rssi);
+ (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
+ rc.rssi);
}
+
+ // New message
+ mavlink_msg_rc_channels_send(_channel,
+ rc.timestamp_publication / 1000,
+ ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
+ ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
+ ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
+ ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
+ ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
+ ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
+ ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
+ ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
+ ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
+ ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
+ ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
+ ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
+ ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
+ ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
+ ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
+ ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
+ ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
+ ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
+ rc.channel_count,
+ rc.rssi);
}
}
};
@@ -1137,36 +1324,46 @@ protected:
class MavlinkStreamManualControl : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamManualControl::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "MANUAL_CONTROL";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamManualControl();
}
private:
MavlinkOrbSubscription *manual_sub;
- struct manual_control_setpoint_s *manual;
+ uint64_t manual_time;
protected:
+ explicit MavlinkStreamManualControl() : MavlinkStream(),
+ manual_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
- manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (manual_sub->update(t)) {
+ struct manual_control_setpoint_s manual;
+
+ if (manual_sub->update(&manual_time, &manual)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
- manual->x * 1000,
- manual->y * 1000,
- manual->z * 1000,
- manual->r * 1000,
+ manual.x * 1000,
+ manual.y * 1000,
+ manual.z * 1000,
+ manual.r * 1000,
0);
}
}
@@ -1176,37 +1373,47 @@ protected:
class MavlinkStreamOpticalFlow : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamOpticalFlow::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "OPTICAL_FLOW";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamOpticalFlow();
}
private:
MavlinkOrbSubscription *flow_sub;
- struct optical_flow_s *flow;
+ uint64_t flow_time;
protected:
+ explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+ flow_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
- flow = (struct optical_flow_s *)flow_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (flow_sub->update(t)) {
+ struct optical_flow_s flow;
+
+ if (flow_sub->update(&flow_time, &flow)) {
mavlink_msg_optical_flow_send(_channel,
- flow->timestamp,
- flow->sensor_id,
- flow->flow_raw_x, flow->flow_raw_y,
- flow->flow_comp_x_m, flow->flow_comp_y_m,
- flow->quality,
- flow->ground_distance_m);
+ flow.timestamp,
+ flow.sensor_id,
+ flow.flow_raw_x, flow.flow_raw_y,
+ flow.flow_comp_x_m, flow.flow_comp_y_m,
+ flow.quality,
+ flow.ground_distance_m);
}
}
};
@@ -1214,47 +1421,57 @@ protected:
class MavlinkStreamAttitudeControls : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitudeControls::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE_CONTROLS";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeControls();
}
private:
MavlinkOrbSubscription *att_ctrl_sub;
- struct actuator_controls_s *att_ctrl;
+ uint64_t att_ctrl_time;
protected:
+ explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+ att_ctrl_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
- att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_ctrl_sub->update(t)) {
+ struct actuator_controls_s att_ctrl;
+
+ if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"rll ctrl ",
- att_ctrl->control[0]);
+ att_ctrl.control[0]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"ptch ctrl ",
- att_ctrl->control[1]);
+ att_ctrl.control[1]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"yaw ctrl ",
- att_ctrl->control[2]);
+ att_ctrl.control[2]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"thr ctrl ",
- att_ctrl->control[3]);
+ att_ctrl.control[3]);
}
}
};
@@ -1262,37 +1479,47 @@ protected:
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamNamedValueFloat::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "NAMED_VALUE_FLOAT";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamNamedValueFloat();
}
private:
MavlinkOrbSubscription *debug_sub;
- struct debug_key_value_s *debug;
+ uint64_t debug_time;
protected:
+ explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+ debug_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
- debug = (struct debug_key_value_s *)debug_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (debug_sub->update(t)) {
+ struct debug_key_value_s debug;
+
+ if (debug_sub->update(&debug_time, &debug)) {
/* enforce null termination */
- debug->key[sizeof(debug->key) - 1] = '\0';
+ debug.key[sizeof(debug.key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
- debug->timestamp_ms,
- debug->key,
- debug->value);
+ debug.timestamp_ms,
+ debug.key,
+ debug.value);
}
}
};
@@ -1300,33 +1527,37 @@ protected:
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamCameraCapture::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "CAMERA_CAPTURE";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamCameraCapture();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
- (void)status_sub->update(t);
+ struct vehicle_status_s status;
+ (void)status_sub->update(&status);
- if (status->arming_state == ARMING_STATE_ARMED
- || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ if (status.arming_state == ARMING_STATE_ARMED
+ || status.arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
@@ -1341,34 +1572,44 @@ protected:
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamDistanceSensor::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "DISTANCE_SENSOR";
}
- MavlinkStream *new_instance()
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
}
private:
MavlinkOrbSubscription *range_sub;
- struct range_finder_report *range;
+ uint64_t range_time;
protected:
+ explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+ range_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
- range = (struct range_finder_report *)range_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (range_sub->update(t)) {
+ struct range_finder_report range;
+
+ if (range_sub->update(&range_time, &range)) {
uint8_t type;
- switch (range->type) {
+ switch (range.type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
@@ -1378,39 +1619,40 @@ protected:
uint8_t orientation = 0;
uint8_t covariance = 20;
- mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
- range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+ mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+ range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
}
}
};
-MavlinkStream *streams_list[] = {
- new MavlinkStreamHeartbeat(),
- new MavlinkStreamSysStatus(),
- new MavlinkStreamHighresIMU(),
- new MavlinkStreamAttitude(),
- new MavlinkStreamAttitudeQuaternion(),
- new MavlinkStreamVFRHUD(),
- new MavlinkStreamGPSRawInt(),
- new MavlinkStreamGlobalPositionInt(),
- new MavlinkStreamLocalPositionNED(),
- new MavlinkStreamGPSGlobalOrigin(),
- new MavlinkStreamServoOutputRaw(0),
- new MavlinkStreamServoOutputRaw(1),
- new MavlinkStreamServoOutputRaw(2),
- new MavlinkStreamServoOutputRaw(3),
- new MavlinkStreamHILControls(),
- new MavlinkStreamGlobalPositionSetpointInt(),
- new MavlinkStreamLocalPositionSetpoint(),
- new MavlinkStreamRollPitchYawThrustSetpoint(),
- new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
- new MavlinkStreamRCChannelsRaw(),
- new MavlinkStreamManualControl(),
- new MavlinkStreamOpticalFlow(),
- new MavlinkStreamAttitudeControls(),
- new MavlinkStreamNamedValueFloat(),
- new MavlinkStreamCameraCapture(),
- new MavlinkStreamDistanceSensor(),
- new MavlinkStreamViconPositionEstimate(),
+
+StreamListItem *streams_list[] = {
+ new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+ new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
+ new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+ new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+ new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+ new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+ new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+ new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
+ new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
+ new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+ new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
+ new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
+ new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+ new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+ new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
+ new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
+ new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+ new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
+ new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+ new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
nullptr
};
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index b8823263a..ee64d0e42 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -43,6 +43,19 @@
#include "mavlink_stream.h"
-extern MavlinkStream *streams_list[];
+class StreamListItem {
+
+public:
+ MavlinkStream* (*new_instance)();
+ const char* (*get_name)();
+
+ StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+ new_instance(inst),
+ get_name(name) {};
+
+ ~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 21d5219d3..901fa8f05 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -50,50 +50,55 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
_topic(topic),
- _last_check(0),
next(nullptr)
{
- _data = malloc(topic->o_size);
- memset(_data, 0, topic->o_size);
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
close(_fd);
- free(_data);
}
orb_id_t
-MavlinkOrbSubscription::get_topic()
+MavlinkOrbSubscription::get_topic() const
{
return _topic;
}
-void *
-MavlinkOrbSubscription::get_data()
-{
- return _data;
-}
-
bool
-MavlinkOrbSubscription::update(const hrt_abstime t)
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
{
- if (_last_check == t) {
- /* already checked right now, return result of the check */
- return _updated;
+ // TODO this is NOT atomic operation, we can get data newer than time
+ // if topic was published between orb_stat and orb_copy calls.
- } else {
- _last_check = t;
- orb_check(_fd, &_updated);
+ uint64_t time_topic;
+ if (orb_stat(_fd, &time_topic)) {
+ /* error getting last topic publication time */
+ time_topic = 0;
+ }
+
+ if (orb_copy(_topic, _fd, data)) {
+ /* error copying topic data */
+ memset(data, 0, _topic->o_size);
+ return false;
- if (_updated) {
- orb_copy(_topic, _fd, _data);
- _published = true;
+ } else {
+ /* data copied successfully */
+ _published = true;
+ if (time_topic != *time) {
+ *time = time_topic;
return true;
+
+ } else {
+ return false;
}
}
+}
- return false;
+bool
+MavlinkOrbSubscription::update(void* data)
+{
+ return !orb_copy(_topic, _fd, data);
}
bool
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 8c09772c8..71efb43af 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -48,12 +48,26 @@
class MavlinkOrbSubscription
{
public:
- MavlinkOrbSubscription *next; /*< pointer to next subscription in list */
+ MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
~MavlinkOrbSubscription();
- bool update(const hrt_abstime t);
+ /**
+ * Check if subscription updated and get data.
+ *
+ * @return true only if topic was updated and data copied to buffer successfully.
+ * If topic was not updated since last check it will return false but still copy the data.
+ * If no data available data buffer will be filled with zeroes.
+ */
+ bool update(uint64_t *time, void* data);
+
+ /**
+ * Copy topic data to given buffer.
+ *
+ * @return true only if topic data copied successfully.
+ */
+ bool update(void* data);
/**
* Check if the topic has been published.
@@ -62,16 +76,12 @@ public:
* @return true if the topic has been published at least once.
*/
bool is_published();
- void *get_data();
- orb_id_t get_topic();
+ orb_id_t get_topic() const;
private:
- const orb_id_t _topic; /*< topic metadata */
- int _fd; /*< subscription handle */
- bool _published; /*< topic was ever published */
- void *_data; /*< pointer to data buffer */
- hrt_abstime _last_check; /*< time of last check */
- bool _updated; /*< updated on last check */
+ const orb_id_t _topic; ///< topic metadata
+ int _fd; ///< subscription handle
+ bool _published; ///< topic was ever published
};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 33358b7b6..1e217ec70 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -952,7 +952,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
-
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 2979d20de..a41ace48e 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -50,14 +50,6 @@ class MavlinkStream;
class MavlinkStream
{
-private:
- hrt_abstime _last_sent;
-
-protected:
- mavlink_channel_t _channel;
- unsigned int _interval;
-
- virtual void send(const hrt_abstime t) = 0;
public:
MavlinkStream *next;
@@ -71,9 +63,19 @@ public:
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
- virtual MavlinkStream *new_instance() = 0;
+ static MavlinkStream *new_instance();
+ static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
- virtual const char *get_name() = 0;
+ virtual const char *get_name() const = 0;
+
+protected:
+ mavlink_channel_t _channel;
+ unsigned int _interval;
+
+ virtual void send(const hrt_abstime t) = 0;
+
+private:
+ hrt_abstime _last_sent;
};
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index 532eff7aa..a2109f2d8 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -88,7 +88,7 @@ struct mavlink_wpm_storage {
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
- uint32_t timeout;
+ uint32_t action_timeout;
int current_dataman_id;
};
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a3c190c7f..888c8e7f4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -227,6 +227,13 @@ Navigator::task_main()
warnx("Could not clear geofence");
}
+ /* Get the last offboard mission id */
+ persistent_system_state_t sys_state;
+ if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
+ if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1))
+ _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id);
+ }
+
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d25db3a77..4bcaaaa5a 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -99,8 +99,8 @@ struct mission_item_s {
struct mission_s
{
- int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
- unsigned count; /**< count of the missions stored in the datamanager */
+ int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
+ unsigned count; /**< count of the missions stored in the dataman */
int current_index; /**< default -1, start at the one changed latest */
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 15:21:24 +0000