mavlink: move commands stream to mavlink_messages.cpp, bugs fixed

7 files changed, 78 insertions, 149 deletions

diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 374d1511c..d82c2dae7 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -44,7 +44,7 @@
 
 __BEGIN_DECLS
 
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
deleted file mode 100644
index b502c3c86..000000000
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- *   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) :
-	_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
-	_cmd{},
-	_channel(channel),
-	_cmd_time(0)
-{
-}
-
-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
deleted file mode 100644
index abdba34b9..000000000
--- a/src/modules/mavlink/mavlink_commands.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   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 dca76c950..4f4bf8691 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -78,7 +78,6 @@
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 #include "mavlink_rate_limiter.h"
-#include "mavlink_commands.h"
 
 #ifndef MAVLINK_CRC_EXTRA
 #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
@@ -840,7 +839,7 @@ Mavlink::send_statustext(unsigned severity, const char *string)
 			break;
 		}
 
-		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
+		send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext);
 		return OK;
 
 	} else {
@@ -1313,13 +1312,14 @@ Mavlink::task_main(int argc, char *argv[])
 	struct vehicle_status_s status;
 	status_sub->update(&status_time, &status);
 
-	MavlinkCommandsStream commands_stream(this, _channel);
-
 	/* add default streams depending on mode */
 
 	/* HEARTBEAT is constant rate stream, rate never adjusted */
 	configure_stream("HEARTBEAT", 1.0f);
 
+	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
+	configure_stream("COMMAND_LONG", 100.0f);
+
 	/* PARAM_VALUE stream */
 	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
 	_parameters_manager->set_interval(interval_from_rate(30.0f));
@@ -1384,9 +1384,6 @@ Mavlink::task_main(int argc, char *argv[])
 			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)) {
@@ -1469,7 +1466,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 				pthread_mutex_unlock(&_message_buffer_mutex);
 
-				_mavlink_resend_uart(_channel, &msg);
+				// TODO implement message resending
+				//_mavlink_resend_uart(_channel, &msg);
 			}
 		}
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7dc3ebac1..a8b4d11cc 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -312,6 +312,75 @@ protected:
 	}
 };
 
+class MavlinkStreamCommandLong : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamCommandLong::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "COMMAND_LONG";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_COMMAND_LONG;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamCommandLong(mavlink);
+	}
+
+	unsigned get_size() { return 0; }	// commands stream is not regular and not predictable
+
+private:
+	MavlinkOrbSubscription *_cmd_sub;
+	uint64_t _cmd_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamCommandLong(MavlinkStreamCommandLong &);
+	MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &);
+
+protected:
+	explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_cmd_sub(nullptr),
+		_cmd_time(0)
+	{}
+
+	void subscribe() {
+		_cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+	}
+
+	void send(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_command_long_t mavcmd;
+
+				mavcmd.target_system = cmd.target_system;
+				mavcmd.target_component = cmd.target_component;
+				mavcmd.command = cmd.command;
+				mavcmd.confirmation = cmd.confirmation;
+				mavcmd.param1 = cmd.param1;
+				mavcmd.param2 = cmd.param2;
+				mavcmd.param3 = cmd.param3;
+				mavcmd.param4 = cmd.param4;
+				mavcmd.param5 = cmd.param5;
+				mavcmd.param6 = cmd.param6;
+				mavcmd.param7 = cmd.param7;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd);
+			}
+		}
+	}
+};
 
 class MavlinkStreamSysStatus : public MavlinkStream
 {
@@ -1926,6 +1995,7 @@ protected:
 
 StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::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),
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 4e8cd4ba4..cd5f53d88 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -147,7 +147,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 	if (_send_all_index >= 0) {
 		send_param(param_for_index(_send_all_index));
 		_send_all_index++;
-		if (_send_all_index >= param_count()) {
+		if (_send_all_index >= (int) param_count()) {
 			_send_all_index = -1;
 		}
 	}
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 08faf102a..91fdd6154 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -45,7 +45,6 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp \
 			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink