dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Anton Babushkin 2014-07-23 15:37:56 +02:00
parent a5f2d1b066
commit d70b21c51a
7 changed files with 78 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -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

73
src/modules/mavlink/mavlink_commands.cpp
View File

@ -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);
}
}
}

65
src/modules/mavlink/mavlink_commands.h
View File

@ -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_ */

14
src/modules/mavlink/mavlink_main.cpp
View File

@ -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);
}
}

70
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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),

2
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -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;
}
}

1
src/modules/mavlink/module.mk
View File

@ -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