aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c6
-rw-r--r--src/modules/mavlink/mavlink_commands.cpp70
-rw-r--r--src/modules/mavlink/mavlink_commands.h65
-rw-r--r--src/modules/mavlink/mavlink_main.cpp527
-rw-r--r--src/modules/mavlink/mavlink_main.h69
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp1113
-rw-r--r--src/modules/mavlink/mavlink_messages.h15
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.cpp53
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.h30
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp147
-rw-r--r--src/modules/mavlink/mavlink_receiver.h5
-rw-r--r--src/modules/mavlink/mavlink_stream.cpp4
-rw-r--r--src/modules/mavlink/mavlink_stream.h30
-rw-r--r--src/modules/mavlink/module.mk7
14 files changed, 1512 insertions, 629 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index ad435b251..e49288a74 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
+/**
+ * Use/Accept HIL GPS message (even if not in HIL mode)
+ * If set to 1 incomming HIL GPS messages are parsed.
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
mavlink_system_t mavlink_system = {
100,
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 000000000..fccd4d9a5
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_commands.cpp
+ * Mavlink commands stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_commands.h"
+
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
+{
+ _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+}
+
+void
+MavlinkCommandsStream::update(const hrt_abstime 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) {
+ 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);
+ }
+ }
+}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
new file mode 100644
index 000000000..abdba34b9
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_commands.h
+ * Mavlink commands stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_COMMANDS_H_
+#define MAVLINK_COMMANDS_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+
+class Mavlink;
+class MavlinkCommansStream;
+
+#include "mavlink_main.h"
+
+class MavlinkCommandsStream
+{
+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);
+ void update(const hrt_abstime t);
+};
+
+#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 16c27baa9..ee5e7aa3a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -81,6 +81,7 @@
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
#include "mavlink_rate_limiter.h"
+#include "mavlink_commands.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -104,7 +105,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
-static uint64_t last_write_times[6] = {0};
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
@@ -148,10 +150,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
instance = Mavlink::get_instance(6);
break;
#endif
- }
-
- /* no valid instance, bail */
- if (!instance) {
+ default:
return;
}
@@ -166,54 +165,87 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
- && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+ && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+
+ /* Disable hardware flow control:
+ * if no successful write since a defined time
+ * and if the last try was not the last successful write
+ */
+ if (last_write_try_times[(unsigned)channel] != 0 &&
+ hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+ last_write_success_times[(unsigned)channel] !=
+ last_write_try_times[(unsigned)channel])
+ {
+ warnx("DISABLING HARDWARE FLOW CONTROL");
+ instance->enable_flow_control(false);
+ }
+
+ }
- if (buf_free == 0) {
+ /* If the wait until transmit flag is on, only transmit after we've received messages.
+ Otherwise, transmit all the time. */
+ if (instance->should_transmit()) {
+ last_write_try_times[(unsigned)channel] = hrt_absolute_time();
- if (last_write_times[(unsigned)channel] != 0 &&
- hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
+ /* check if there is space in the buffer, let it overflow else */
+ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
- warnx("DISABLING HARDWARE FLOW CONTROL");
- instance->enable_flow_control(false);
+ if (buf_free < desired) {
+ /* we don't want to send anything just in half, so return */
+ instance->count_txerr();
+ return;
}
+ }
+ ssize_t ret = write(uart, ch, desired);
+ if (ret != desired) {
+ instance->count_txerr();
} else {
-
- /* apparently there is space left, although we might be
- * partially overflooding the buffer already */
- last_write_times[(unsigned)channel] = hrt_absolute_time();
+ last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
- ssize_t ret = write(uart, ch, desired);
- if (ret != desired) {
- // XXX do something here, but change to using FIONWRITE and OS buf size for detection
- }
}
static void usage(void);
Mavlink::Mavlink() :
- next(nullptr),
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
+ next(nullptr),
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
+ _use_hil_gps(false),
_is_usb_uart(false),
+ _wait_to_transmit(false),
+ _received_messages(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_pub(-1),
+ _mode(MAVLINK_MODE_NORMAL),
+ _total_counter(0),
+ _verbose(false),
+ _forwarding_on(false),
+ _passing_on(false),
+ _uart_fd(-1),
_mavlink_param_queue_index(0),
_subscribe_to_stream(nullptr),
_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"))
+ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+ _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
_wpm = &_wpm_s;
mission.count = 0;
@@ -261,12 +293,12 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
-
}
Mavlink::~Mavlink()
{
perf_free(_loop_perf);
+ perf_free(_txerr_perf);
if (_task_running) {
/* task wakes up every 10ms or so at the longest */
@@ -292,6 +324,12 @@ Mavlink::~Mavlink()
}
void
+Mavlink::count_txerr()
+{
+ perf_count(_txerr_perf);
+}
+
+void
Mavlink::set_mode(enum MAVLINK_MODE mode)
{
_mode = mode;
@@ -394,6 +432,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
return false;
}
+void
+Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
+{
+
+ Mavlink *inst;
+ LL_FOREACH(_mavlink_instances, inst) {
+ if (inst != self) {
+ inst->pass_message(msg);
+ }
+ }
+}
+
int
Mavlink::get_uart_fd(unsigned index)
{
@@ -459,39 +509,41 @@ 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;
- 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");
- 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);
+
+ _use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
@@ -550,6 +602,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* open uart */
_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
+ if (_uart_fd < 0) {
+ return _uart_fd;
+ }
+
+
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
@@ -690,9 +747,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
- static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+ char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
- static mavlink_message_t tx_msg;
+ mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
@@ -743,9 +800,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
- /* Start sending parameters */
- mavlink_pm_start_queued_send();
- mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+ mavlink_param_request_list_t req;
+ mavlink_msg_param_request_list_decode(msg, &req);
+ if (req.target_system == mavlink_system.sysid &&
+ (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
+ /* Start sending parameters */
+ mavlink_pm_start_queued_send();
+ mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+ }
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
@@ -767,7 +829,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 {
@@ -808,10 +870,10 @@ void Mavlink::publish_mission()
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
- _mission_pub = orb_advertise(ORB_ID(mission), &mission);
+ _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
- orb_publish(ORB_ID(mission), _mission_pub, &mission);
+ orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
}
}
@@ -844,11 +906,16 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
- mission_item->pitch_min = mavlink_mission_item->param2;
+ mission_item->pitch_min = mavlink_mission_item->param1;
+ break;
+ case MAV_CMD_DO_JUMP:
+ mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+ mission_item->do_jump_current_count = 0;
+ mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
-
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
+ mission_item->time_inside = mavlink_mission_item->param1;
break;
}
@@ -857,11 +924,13 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
- mission_item->time_inside = mavlink_mission_item->param1;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
+ /* reset DO_JUMP count */
+ mission_item->do_jump_current_count = 0;
+
return OK;
}
@@ -876,11 +945,17 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
- mavlink_mission_item->param2 = mission_item->pitch_min;
+ mavlink_mission_item->param1 = mission_item->pitch_min;
+ break;
+
+ case NAV_CMD_DO_JUMP:
+ mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+ mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
+ mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
@@ -891,7 +966,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
- mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
@@ -907,6 +981,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
state->current_partner_compid = 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;
}
@@ -955,8 +1030,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-
- if (_verbose) { warnx("ERROR: index out of bounds"); }
}
}
@@ -1007,6 +1080,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
} 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); }
}
@@ -1022,13 +1096,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
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); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-
- if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
}
}
@@ -1059,11 +1132,13 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
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); }
-
_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) {
+ /* 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);
}
}
@@ -1091,8 +1166,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-
- if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
}
break;
@@ -1116,21 +1189,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-
- if (_verbose) { warnx("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"); }
-
}
-
- } else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
- if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
}
break;
@@ -1160,14 +1223,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-
- if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
-
- } else {
- mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
-
- if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
}
break;
@@ -1184,8 +1240,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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;
}
@@ -1196,15 +1250,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
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 (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
- if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
-
break;
}
@@ -1212,17 +1264,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == _wpm->current_wp_id) {
- 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); }
+ if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == _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); }
+ if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} 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); }
-
break;
}
@@ -1230,8 +1280,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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;
}
@@ -1245,7 +1293,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
- if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
+ mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
}
@@ -1256,12 +1304,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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); }
-
- } else {
-
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
- if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
@@ -1285,15 +1327,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
}
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"); }
+ mavlink_missionlib_send_gcs_string("WP COUNT 0");
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;
@@ -1307,25 +1345,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
- if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
-
} else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
- 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 with WP");
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-
- if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
-
- } else {
-
- mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
-
- if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
@@ -1347,7 +1373,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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);
break;
}
@@ -1355,12 +1380,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wp.seq >= _wpm->current_count) {
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);
+ mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
@@ -1393,6 +1417,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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;
}
@@ -1424,11 +1449,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
-
- } else {
- mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
- if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@@ -1464,13 +1484,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
-
-
- } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
- mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
-
- if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@@ -1486,9 +1499,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
- 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);
}
@@ -1498,6 +1512,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
+ statustext.severity = MAV_SEVERITY_INFO;
+
int i = 0;
while (i < len - 1) {
@@ -1560,31 +1576,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
/* delete stream */
LL_DELETE(_streams, stream);
delete stream;
+ warnx("deleted stream %s", stream->get_name());
}
return OK;
}
}
- 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();
- stream->set_channel(get_channel());
- stream->set_interval(interval);
- stream->subscribe(this);
- LL_APPEND(_streams, stream);
- return OK;
- }
- }
-
- } else {
- /* stream not found, nothing to disable */
+ if (interval == 0) {
+ /* stream was not active and is requested to be disabled, do nothing */
return OK;
}
+ /* 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();
+ stream->set_channel(get_channel());
+ stream->set_interval(interval);
+ stream->subscribe(this);
+ LL_APPEND(_streams, stream);
+
+ return OK;
+ }
+ }
+
+ /* if we reach here, the stream list does not contain the stream */
+ warnx("stream %s not found", stream_name);
+
return ERROR;
}
@@ -1617,6 +1638,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
}
int
+Mavlink::message_buffer_init(int size)
+{
+ _message_buffer.size = size;
+ _message_buffer.write_ptr = 0;
+ _message_buffer.read_ptr = 0;
+ _message_buffer.data = (char*)malloc(_message_buffer.size);
+ return (_message_buffer.data == 0) ? ERROR : OK;
+}
+
+void
+Mavlink::message_buffer_destroy()
+{
+ _message_buffer.size = 0;
+ _message_buffer.write_ptr = 0;
+ _message_buffer.read_ptr = 0;
+ free(_message_buffer.data);
+}
+
+int
+Mavlink::message_buffer_count()
+{
+ int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+ if (n < 0) {
+ n += _message_buffer.size;
+ }
+
+ return n;
+}
+
+int
+Mavlink::message_buffer_is_empty()
+{
+ return _message_buffer.read_ptr == _message_buffer.write_ptr;
+}
+
+
+bool
+Mavlink::message_buffer_write(void *ptr, int size)
+{
+ // bytes available to write
+ int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
+
+ if (available < 0) {
+ available += _message_buffer.size;
+ }
+
+ if (size > available) {
+ // buffer overflow
+ return false;
+ }
+
+ char *c = (char *) ptr;
+ int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer
+
+ if (n < size) {
+ // message goes over end of the buffer
+ memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
+ _message_buffer.write_ptr = 0;
+
+ } else {
+ n = 0;
+ }
+
+ // now: n = bytes already written
+ int p = size - n; // number of bytes to write
+ memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
+ _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
+ return true;
+}
+
+int
+Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
+{
+ // bytes available to read
+ int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+ if (available == 0) {
+ return 0; // buffer is empty
+ }
+
+ int n = 0;
+
+ if (available > 0) {
+ // read pointer is before write pointer, all available bytes can be read
+ n = available;
+ *is_part = false;
+
+ } else {
+ // read pointer is after write pointer, read bytes from read_ptr to end of the buffer
+ n = _message_buffer.size - _message_buffer.read_ptr;
+ *is_part = _message_buffer.write_ptr > 0;
+ }
+
+ *ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
+ return n;
+}
+
+void
+Mavlink::message_buffer_mark_read(int n)
+{
+ _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
+}
+
+void
+Mavlink::pass_message(mavlink_message_t *msg)
+{
+ if (_passing_on) {
+ /* size is 8 bytes plus variable payload */
+ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
+ pthread_mutex_lock(&_message_buffer_mutex);
+ message_buffer_write(msg, size);
+ pthread_mutex_unlock(&_message_buffer_mutex);
+ }
+}
+
+
+
+int
Mavlink::task_main(int argc, char *argv[])
{
int ch;
@@ -1632,7 +1772,7 @@ Mavlink::task_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
- while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
+ while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,10 +1812,22 @@ Mavlink::task_main(int argc, char *argv[])
break;
+ case 'f':
+ _forwarding_on = true;
+ break;
+
+ case 'p':
+ _passing_on = true;
+ break;
+
case 'v':
_verbose = true;
break;
+ case 'w':
+ _wait_to_transmit = true;
+ break;
+
default:
err_flag = true;
break;
@@ -1740,6 +1892,17 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&_logbuffer, 5);
+ /* 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(300)) {
+ errx(1, "can't allocate message buffer, exiting");
+ }
+
+ /* initialize message buffer mutex */
+ pthread_mutex_init(&_message_buffer_mutex, NULL);
+ }
+
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
@@ -1762,9 +1925,14 @@ 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;
+ status_sub->update(&status_time, &status);
- struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+ MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
@@ -1785,6 +1953,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+ configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
@@ -1818,28 +1987,31 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
- if (param_sub->update(t)) {
+ if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */
mavlink_update_system();
}
- if (status_sub->update(t)) {
+ if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
- set_hil_enabled(status->hil_state == HIL_STATE_ON);
+ set_hil_enabled(status.hil_state == HIL_STATE_ON);
}
+ /* update commands stream */
+ commands_stream.update(t);
+
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
- warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate);
+ warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
} else {
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
}
} else {
- warnx("stream %s not found", _subscribe_to_stream, _device_name);
+ warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
}
delete _subscribe_to_stream;
@@ -1886,6 +2058,37 @@ Mavlink::task_main(int argc, char *argv[])
}
}
+ /* pass messages from other UARTs */
+ if (_passing_on) {
+
+ bool is_part;
+ void *read_ptr;
+
+ /* guard get ptr by mutex */
+ pthread_mutex_lock(&_message_buffer_mutex);
+ int available = message_buffer_get_ptr(&read_ptr, &is_part);
+ pthread_mutex_unlock(&_message_buffer_mutex);
+
+ if (available > 0) {
+ /* write first part of buffer */
+ _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+ message_buffer_mark_read(available);
+
+ /* write second part of buffer if there is some */
+ if (is_part) {
+ /* guard get ptr by mutex */
+ pthread_mutex_lock(&_message_buffer_mutex);
+ available = message_buffer_get_ptr(&read_ptr, &is_part);
+ pthread_mutex_unlock(&_message_buffer_mutex);
+
+ _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+ message_buffer_mark_read(available);
+ }
+ }
+ }
+
+
+
perf_end(_loop_perf);
}
@@ -1930,6 +2133,10 @@ Mavlink::task_main(int argc, char *argv[])
/* close mavlink logging device */
close(_mavlink_fd);
+ if (_passing_on) {
+ message_buffer_destroy();
+ pthread_mutex_destroy(&_message_buffer_mutex);
+ }
/* destroy log buffer */
mavlink_logbuffer_destroy(&_logbuffer);
@@ -1944,11 +2151,20 @@ int Mavlink::start_helper(int argc, char *argv[])
/* create the instance in task context */
Mavlink *instance = new Mavlink();
- /* this will actually only return once MAVLink exits */
- int res = instance->task_main(argc, argv);
+ int res;
- /* delete instance on main thread end */
- delete instance;
+ if (!instance) {
+
+ /* out of memory */
+ res = -ENOMEM;
+ warnx("OUT OF MEM");
+ } else {
+ /* this will actually only return once MAVLink exits */
+ res = instance->task_main(argc, argv);
+
+ /* delete instance on main thread end */
+ delete instance;
+ }
return res;
}
@@ -1971,7 +2187,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
- 2048,
+ 1950,
(main_t)&Mavlink::start_helper,
(const char **)argv);
@@ -2010,7 +2226,6 @@ Mavlink::stream(int argc, char *argv[])
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
const char *stream_name = nullptr;
- int ch;
argc -= 2;
argv += 2;
@@ -2047,7 +2262,7 @@ Mavlink::stream(int argc, char *argv[])
i++;
}
- if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
+ if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
if (inst != nullptr) {
@@ -2069,7 +2284,7 @@ Mavlink::stream(int argc, char *argv[])
static void usage()
{
- warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]");
+ warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
}
int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 427b9ad35..f74b7daa7 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
+ uint64_t timestamp_last_send_request;
uint32_t timeout;
int current_dataman_id;
};
@@ -138,6 +139,8 @@ public:
static bool instance_exists(const char *device_name, Mavlink *self);
+ static void forward_message(mavlink_message_t *msg, Mavlink *self);
+
static int get_uart_fd(unsigned index);
int get_uart_fd();
@@ -153,10 +156,14 @@ public:
void set_mode(enum MAVLINK_MODE);
enum MAVLINK_MODE get_mode() { return _mode; }
- bool get_hil_enabled() { return _hil_enabled; };
+ bool get_hil_enabled() { return _hil_enabled; }
+
+ bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
+ bool get_forwarding_on() { return _forwarding_on; }
+
/**
* Handle waypoint related messages.
*/
@@ -198,6 +205,23 @@ public:
bool _task_should_exit; /**< if true, mavlink task should exit */
+ int get_mavlink_fd() { return _mavlink_fd; }
+
+ MavlinkStream * get_streams() { return _streams; } const
+
+
+ /* Functions for waiting to start transmission until message received. */
+ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+ bool get_has_received_messages() { return _received_messages; }
+ void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+ bool get_wait_to_transmit() { return _wait_to_transmit; }
+ bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
+ /**
+ * Count a transmision error
+ */
+ void count_txerr();
+
protected:
Mavlink *next;
@@ -207,11 +231,12 @@ private:
int _mavlink_fd;
bool _task_running;
- perf_counter_t _loop_perf; /**< loop performance counter */
-
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
+ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
+ bool _wait_to_transmit; /**< Wait to transmit until received messages. */
+ bool _received_messages; /**< Whether we've received valid mavlink messages. */
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
@@ -220,7 +245,6 @@ private:
orb_advert_t _mission_pub;
struct mission_s mission;
- uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
@@ -236,6 +260,8 @@ private:
mavlink_wpm_storage *_wpm;
bool _verbose;
+ bool _forwarding_on;
+ bool _passing_on;
int _uart_fd;
int _baudrate;
int _datarate;
@@ -254,6 +280,25 @@ private:
bool _flow_control_enabled;
+ struct mavlink_message_buffer {
+ int write_ptr;
+ int read_ptr;
+ int size;
+ char *data;
+ };
+ mavlink_message_buffer _message_buffer;
+
+ pthread_mutex_t _message_buffer_mutex;
+
+ perf_counter_t _loop_perf; /**< loop performance counter */
+ perf_counter_t _txerr_perf; /**< TX error 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.
*
@@ -316,6 +361,22 @@ private:
int configure_stream(const char *stream_name, const float rate);
+ int message_buffer_init(int size);
+
+ void message_buffer_destroy();
+
+ int message_buffer_count();
+
+ int message_buffer_is_empty();
+
+ bool message_buffer_write(void *ptr, int size);
+
+ int message_buffer_get_ptr(void **ptr, bool *is_part);
+
+ void message_buffer_mark_read(int n);
+
+ void pass_message(mavlink_message_t *msg);
+
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5b285dc9b..0aa4e005a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -72,6 +72,9 @@
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
+
+#include <systemlib/err.h>
#include "mavlink_messages.h"
@@ -117,46 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
union px4_custom_mode custom_mode;
custom_mode.data = 0;
- if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
- /* use main state when navigator is not active */
- if (status->main_state == MAIN_STATE_MANUAL) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-
- } else if (status->main_state == MAIN_STATE_SEATBELT) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-
- } else if (status->main_state == MAIN_STATE_EASY) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+ switch (status->nav_state) {
- } else if (status->main_state == MAIN_STATE_AUTO) {
- *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+ case NAVIGATION_STATE_MANUAL:
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+ | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+ break;
+
+ case NAVIGATION_STATE_ACRO:
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+ break;
+
+ case NAVIGATION_STATE_ALTCTL:
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+ break;
+
+ case NAVIGATION_STATE_POSCTL:
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+ break;
+
+ case NAVIGATION_STATE_AUTO_MISSION:
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
- }
-
- } else {
- /* use navigation state when navigator is active */
- *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-
- if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+ break;
- } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+ case NAVIGATION_STATE_AUTO_LOITER:
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+ break;
- } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-
- } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+ case NAVIGATION_STATE_AUTO_RTL:
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+ break;
- } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+ case NAVIGATION_STATE_LAND:
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
- }
+ break;
+
+ case NAVIGATION_STATE_AUTO_RTGS:
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
+ | MAV_MODE_FLAG_STABILIZE_ENABLED
+ | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
+ break;
+
+ case NAVIGATION_STATE_TERMINATION:
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+ break;
+
}
*mavlink_custom_mode = custom_mode.data;
@@ -188,7 +222,12 @@ 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";
}
@@ -198,45 +237,40 @@ public:
return MAVLINK_MSG_ID_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);
+ }
}
};
@@ -244,7 +278,12 @@ 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";
}
@@ -254,39 +293,40 @@ public:
return MAVLINK_MSG_ID_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 * 1000.0f,
- status->battery_remaining,
- 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);
+ }
}
};
@@ -294,11 +334,12 @@ 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";
}
@@ -308,14 +349,14 @@ public:
return MAVLINK_MSG_ID_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;
@@ -323,48 +364,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);
}
}
@@ -374,7 +424,12 @@ 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";
}
@@ -384,29 +439,34 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -415,7 +475,12 @@ 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";
}
@@ -425,34 +490,39 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -461,7 +531,13 @@ 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";
}
@@ -471,66 +547,71 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -539,7 +620,12 @@ 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";
}
@@ -549,36 +635,41 @@ public:
return MAVLINK_MSG_ID_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_m),
- cm_uint16_from_m_float(gps->epv_m),
- 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);
}
}
};
@@ -587,7 +678,12 @@ 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";
}
@@ -597,44 +693,49 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -643,7 +744,12 @@ 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";
}
@@ -653,33 +759,94 @@ public:
return MAVLINK_MSG_ID_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);
+ }
+ }
+};
+
+
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamViconPositionEstimate::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "VICON_POSITION_ESTIMATE";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+ }
+
+ static MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamViconPositionEstimate();
+ }
+
+private:
+ MavlinkOrbSubscription *pos_sub;
+ 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));
+ }
+
+ void send(const hrt_abstime 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);
}
}
};
@@ -688,7 +855,12 @@ 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";
}
@@ -698,45 +870,44 @@ public:
return MAVLINK_MSG_ID_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();
}
uint8_t get_id()
@@ -744,24 +915,37 @@ public:
return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
}
- 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[] = {
@@ -771,24 +955,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]);
}
}
};
@@ -797,7 +982,12 @@ 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";
}
@@ -807,46 +997,51 @@ public:
return MAVLINK_MSG_ID_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 = status_sub->update(t);
- updated |= pos_sp_triplet_sub->update(t);
- updated |= act_sub->update(t);
+ struct vehicle_status_s status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
+ struct actuator_outputs_s act;
- if (updated) {
+ 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)) {
/* 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 ||
@@ -875,7 +1070,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 */
@@ -905,12 +1100,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);
}
}
@@ -929,7 +1124,12 @@ 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";
}
@@ -939,31 +1139,31 @@ public:
return MAVLINK_MSG_ID_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)
{
- if (pos_sp_triplet_sub->update(t)) {
+ 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));
+ (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));
}
}
};
@@ -972,7 +1172,12 @@ 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";
}
@@ -982,31 +1187,36 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1015,7 +1225,12 @@ 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";
}
@@ -1025,31 +1240,36 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1058,7 +1278,12 @@ 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";
}
@@ -1068,31 +1293,36 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1101,7 +1331,12 @@ 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";
}
@@ -1111,42 +1346,72 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1155,7 +1420,12 @@ 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";
}
@@ -1165,31 +1435,36 @@ public:
return MAVLINK_MSG_ID_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->roll * 1000,
- manual->pitch * 1000,
- manual->yaw * 1000,
- manual->throttle * 1000,
+ manual.x * 1000,
+ manual.y * 1000,
+ manual.z * 1000,
+ manual.r * 1000,
0);
}
}
@@ -1199,7 +1474,12 @@ 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";
}
@@ -1209,32 +1489,37 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1242,7 +1527,12 @@ 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";
}
@@ -1252,42 +1542,47 @@ public:
return 0;
}
- 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]);
}
}
};
@@ -1295,7 +1590,12 @@ 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";
}
@@ -1305,32 +1605,37 @@ public:
return MAVLINK_MSG_ID_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);
}
}
};
@@ -1338,7 +1643,12 @@ 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";
}
@@ -1348,64 +1658,127 @@ public:
return 0;
}
- 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, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
- mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+ }
+ }
+};
+
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamDistanceSensor::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "DISTANCE_SENSOR";
+ }
+
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+ }
+
+ static MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamDistanceSensor();
+ }
+
+private:
+ MavlinkOrbSubscription *range_sub;
+ 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));
+ }
+
+ void send(const hrt_abstime t)
+ {
+ struct range_finder_report range;
+
+ if (range_sub->update(&range_time, &range)) {
+
+ uint8_t type;
+
+ switch (range.type) {
+ case RANGE_FINDER_TYPE_LASER:
+ type = MAV_DISTANCE_SENSOR_LASER;
+ break;
+ }
+
+ uint8_t id = 0;
+ 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);
}
}
};
-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(),
+
+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, &MavlinkStreamRCChannelsRaw::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 d432edd2b..3807323c2 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -44,56 +44,65 @@
#include <uORB/uORB.h>
#include <stdio.h>
+#include <systemlib/err.h>
+
#include "mavlink_orb_subscription.h"
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);
}
-const orb_id_t
-MavlinkOrbSubscription::get_topic()
+orb_id_t
+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 (time_topic != *time) {
- if (_updated) {
- orb_copy(_topic, _fd, _data);
+ if (orb_copy(_topic, _fd, data)) {
+ /* error copying topic data */
+ memset(data, 0, _topic->o_size);
+ //warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
+ return false;
+
+ } else {
+ /* data copied successfully */
_published = true;
+ *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 5c6543e81..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();
- const 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 cf8b23bc3..425fc79a5 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
- _manual_sub(-1),
-
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -108,6 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
+ _telemetry_heartbeat_time(0),
+ _radio_status_available(false),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
@@ -152,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ handle_message_heartbeat(msg);
+ break;
+
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
handle_message_request_data_stream(msg);
break;
@@ -166,6 +170,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
+ *
+ * Accept HIL GPS messages if use_hil_gps flag is true.
+ * This allows to provide fake gps measurements to the system.
*/
if (_mavlink->get_hil_enabled()) {
switch (msg->msgid) {
@@ -173,10 +180,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_hil_sensor(msg);
break;
- case MAVLINK_MSG_ID_HIL_GPS:
- handle_message_hil_gps(msg);
- break;
-
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
@@ -185,6 +188,23 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
}
}
+
+
+ if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
+ switch (msg->msgid) {
+ case MAVLINK_MSG_ID_HIL_GPS:
+ handle_message_hil_gps(msg);
+ break;
+
+ default:
+ break;
+ }
+
+ }
+
+ /* If we've received a valid message, mark the flag indicating so.
+ This is used in the '-w' command-line flag. */
+ _mavlink->set_has_received_messages(true);
}
void
@@ -207,6 +227,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->_task_should_exit = true;
} else {
+
+ if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+ warnx("ignoring CMD spoofed with same SYS/COMP ID");
+ return;
+ }
+
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
@@ -247,6 +273,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
+ f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -394,6 +421,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
+ tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
@@ -409,6 +437,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
+
+ /* this means that heartbeats alone won't be published to the radio status no more */
+ _radio_status_available = true;
}
void
@@ -417,46 +448,49 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
- /* rc channels */
- {
- struct rc_channels_s rc;
- memset(&rc, 0, sizeof(rc));
-
- rc.timestamp = hrt_absolute_time();
- rc.chan_count = 4;
+ struct manual_control_setpoint_s manual;
+ memset(&manual, 0, sizeof(manual));
- rc.chan[0].scaled = man.x / 1000.0f;
- rc.chan[1].scaled = man.y / 1000.0f;
- rc.chan[2].scaled = man.r / 1000.0f;
- rc.chan[3].scaled = man.z / 1000.0f;
+ manual.timestamp = hrt_absolute_time();
+ manual.x = man.x / 1000.0f;
+ manual.y = man.y / 1000.0f;
+ manual.r = man.r / 1000.0f;
+ manual.z = man.z / 1000.0f;
- if (_rc_pub < 0) {
- _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
+ if (_manual_pub < 0) {
+ _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
- } else {
- orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
- }
+ } else {
+ orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
+}
- /* manual control */
- {
- struct manual_control_setpoint_s manual;
- memset(&manual, 0, sizeof(manual));
+void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+ mavlink_heartbeat_t hb;
+ mavlink_msg_heartbeat_decode(msg, &hb);
- /* get a copy first, to prevent altering values that are not sent by the mavlink command */
- orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
+ /* ignore own heartbeats, accept only heartbeats from GCS */
+ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+ _telemetry_heartbeat_time = hrt_absolute_time();
- manual.timestamp = hrt_absolute_time();
- manual.roll = man.x / 1000.0f;
- manual.pitch = man.y / 1000.0f;
- manual.yaw = man.r / 1000.0f;
- manual.throttle = man.z / 1000.0f;
+ /* if no radio status messages arrive, lets at least publish that heartbeats were received */
+ if (!_radio_status_available) {
- if (_manual_pub < 0) {
- _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
+ struct telemetry_status_s tstatus;
+ memset(&tstatus, 0, sizeof(tstatus));
- } else {
- orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
+ tstatus.timestamp = _telemetry_heartbeat_time;
+ tstatus.heartbeat_time = _telemetry_heartbeat_time;
+ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+
+ if (_telemetry_status_pub < 0) {
+ _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+ } else {
+ orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+ }
}
}
}
@@ -470,9 +504,10 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
- for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
- if (req.req_stream_id == 0 || req.req_stream_id == streams_list[i]->get_id()) {
- _mavlink->configure_stream_threadsafe(streams_list[i]->get_name(), rate);
+ MavlinkStream *stream;
+ LL_FOREACH(_mavlink->get_streams(), stream) {
+ if (req.req_stream_id == stream->get_id()) {
+ _mavlink->configure_stream_threadsafe(stream->get_name(), rate);
}
}
}
@@ -694,12 +729,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
- hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
- hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+ hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
+ hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.timestamp_variance = timestamp;
hil_gps.s_variance_m_s = 5.0f;
- hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
+ hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
hil_gps.timestamp_velocity = timestamp;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -785,7 +820,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
- hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -793,6 +827,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
+ hil_global_pos.eph = 2.0f;
+ hil_global_pos.epv = 4.0f;
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -804,19 +840,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* local position */
{
+ double lat = hil_state.lat * 1e-7;
+ double lon = hil_state.lon * 1e-7;
+
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
_hil_local_alt0 = hil_state.alt / 1000.0f;
- map_projection_init(hil_state.lat, hil_state.lon);
+ map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
hil_local_pos.ref_timestamp = timestamp;
- hil_local_pos.ref_lat = hil_state.lat;
- hil_local_pos.ref_lon = hil_state.lon;
+ hil_local_pos.ref_lat = lat;
+ hil_local_pos.ref_lon = lon;
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
- map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
+ map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@@ -904,8 +943,6 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
- _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
@@ -932,6 +969,11 @@ MavlinkReceiver::receive_thread(void *arg)
/* handle packet with parameter component */
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
+
+ if (_mavlink->get_forwarding_on()) {
+ /* forward any messages to other mavlink instances */
+ Mavlink::forward_message(&msg, _mavlink);
+ }
}
}
}
@@ -952,6 +994,8 @@ void *MavlinkReceiver::start_helper(void *context)
rcv->receive_thread(NULL);
delete rcv;
+
+ return nullptr;
}
pthread_t
@@ -969,8 +1013,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
- pthread_attr_setstacksize(&receiveloop_attr, 3000);
-
+ 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_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index eab8f071d..af81e8a31 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,6 +112,7 @@ private:
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
+ void handle_message_heartbeat(mavlink_message_t *msg);
void handle_message_request_data_stream(mavlink_message_t *msg);
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
@@ -139,9 +140,11 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
- int _manual_sub;
+ hrt_abstime _telemetry_heartbeat_time;
+ bool _radio_status_available;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
float _hil_local_alt0;
+ struct map_projection_reference_s _hil_local_proj_ref;
};
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index bb19d7e33..5ec30bd33 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t)
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
+
+ return 0;
}
+
+ return -1;
}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index aa8ca450b..69809a386 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file mavlink_stream.cpp
+ * @file mavlink_stream.h
* Mavlink messages stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
@@ -50,27 +50,33 @@ 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;
MavlinkStream();
- ~MavlinkStream();
+ virtual ~MavlinkStream();
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
+
+ /**
+ * @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;
virtual uint8_t get_id() = 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/module.mk b/src/modules/mavlink/module.mk
index f09efa2e6..f532e26fe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -42,6 +42,11 @@ SRCS += mavlink_main.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
- mavlink_rate_limiter.cpp
+ mavlink_rate_limiter.cpp \
+ mavlink_commands.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION = -Os
+
+MODULE_STACKSIZE = 1024
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-06 16:03:22 +0000