diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/dataman/dataman.c | 54 | ||||
-rw-r--r-- | src/modules/dataman/dataman.h | 26 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_commands.cpp | 35 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_commands.h | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 449 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 30 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 964 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.h | 15 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.cpp | 49 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.h | 30 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 1 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 22 | ||||
-rw-r--r-- | src/modules/mavlink/waypoints.h | 2 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 7 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission.h | 4 |
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(¶m_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, ¶m); 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 */ }; |