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

mavlink: new message sending API, includes fixed

This commit is contained in:
Anton Babushkin 2014-07-23 11:11:49 +02:00
parent 344a34bb72
commit a5f2d1b066
9 changed files with 1716 additions and 1877 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/mavlink/mavlink_ftp.h
View File

@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
class MavlinkFTP
{

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

@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr;
static struct file_operations fops;
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
/**
* mavlink app start / stop handling function
@ -130,6 +131,7 @@ Mavlink::Mavlink() :
_subscriptions(nullptr),
_streams(nullptr),
_mission_manager(nullptr),
_parameters_manager(nullptr),
_mission_pub(-1),
_mission_result_sub(-1),
_mode(MAVLINK_MODE_NORMAL),
@ -145,6 +147,7 @@ Mavlink::Mavlink() :
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(0.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
bool
Mavlink::check_can_send(const int prio, const unsigned packet_len)
void
Mavlink::send_message(const uint8_t msgid, const void *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
/*
* Check if the OS buffer is full and disable HW
* flow control if it continues to be full
*/
int buf_free = 0;
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
if (get_flow_control_enabled()
&& ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (get_flow_control_enabled() && 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
@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const unsigned packet_len)
}
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (should_transmit()) {
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) {
warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio);
/* we don't want to send anything just in half, so return */
count_txerr();
count_txerrbytes(packet_len);
return true;
}
}
return false;
}
void
Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (!check_can_send(prio, packet_len)) {
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free >= TX_BUFFER_GAP) {
warnx("SKIP msgid %i, %i bytes", msgid, packet_len);
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
buf[0] = MAVLINK_STX;
buf[1] = payload_len;
@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
/* checksum */
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
#endif
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio)
/* send message to UART */
ssize_t ret = write(_uart_fd, buf, packet_len);
if (ret != packet_len) {
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
@ -783,7 +777,7 @@ Mavlink::handle_message(const mavlink_message_t *msg)
_mission_manager->handle_message(msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(_channel, msg);
_parameters_manager->handle_message(msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
@ -791,163 +785,6 @@ Mavlink::handle_message(const mavlink_message_t *msg)
}
}
int
Mavlink::mavlink_pm_queued_send()
{
if (_mavlink_param_queue_index < param_count()) {
mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
_mavlink_param_queue_index++;
return 0;
} else {
return 1;
}
}
void Mavlink::mavlink_pm_start_queued_send()
{
_mavlink_param_queue_index = 0;
}
int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
{
return mavlink_pm_send_param(param_for_index(index));
}
int Mavlink::mavlink_pm_send_param_for_name(const char *name)
{
return mavlink_pm_send_param(param_find(name));
}
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
/* copy parameter name */
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT;
} else {
mavlink_type = MAVLINK_TYPE_FLOAT;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
int ret;
if ((ret = param_get(param, &val_buf)) != OK) {
return ret;
}
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
_channel,
&tx_msg,
name_buf,
val_buf,
mavlink_type,
param_count(),
param_get_index(param));
send_message(&tx_msg);
return OK;
}
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_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();
send_statustext_info("[pm] sending list");
}
} break;
case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t mavlink_param_set;
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
if (mavlink_param_set.target_system == mavlink_system.sysid
&& ((mavlink_param_set.target_component == mavlink_system.compid)
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown: %s", name);
send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(mavlink_param_set.param_value));
mavlink_pm_send_param(param);
}
}
}
} break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
if (mavlink_param_request_read.target_system == mavlink_system.sysid
&& ((mavlink_param_request_read.target_component == mavlink_system.compid)
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
/* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
mavlink_pm_send_param_for_name(name);
} else {
/* when index is >= 0, send this parameter again */
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
}
}
} break;
}
}
int
Mavlink::send_statustext_info(const char *string)
{
@ -1031,11 +868,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
return sub_new;
}
unsigned int
Mavlink::interval_from_rate(float rate)
{
return (rate > 0.0f) ? (1000000.0f / rate) : 0;
}
int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
/* calculate interval in us, 0 means disabled stream */
unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
unsigned int interval = interval_from_rate(rate);
/* search if stream exists */
MavlinkStream *stream;
@ -1066,10 +909,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
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 = streams_list[i]->new_instance(this);
stream->set_interval(interval);
stream->subscribe(this);
stream->subscribe();
LL_APPEND(_streams, stream);
return OK;
@ -1267,7 +1109,26 @@ Mavlink::pass_message(const mavlink_message_t *msg)
float
Mavlink::get_rate_mult()
{
return _datarate / 1000.0f;
return _rate_mult;
}
void
Mavlink::update_rate_mult()
{
float const_rate = 0.0f;
float rate = 0.0f;
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
if (stream->const_rate()) {
const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
} else {
rate += stream->get_size() * 1000000.0f / stream->get_interval();
}
}
_rate_mult = ((float)_datarate - const_rate) / (rate - const_rate);
}
int
@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
update_rate_mult();
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = get_rate_mult();
/* add default streams depending on mode */
/* HEARTBEAT is constant rate stream, rate never adjusted */
configure_stream("HEARTBEAT", 1.0f);
/* PARAM_VALUE stream */
_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
_parameters_manager->set_interval(interval_from_rate(30.0f));
LL_APPEND(_streams, _parameters_manager);
switch (_mode) {
case MAVLINK_MODE_NORMAL:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
configure_stream("ATTITUDE", 10.0f * rate_mult);
configure_stream("VFR_HUD", 8.0f * rate_mult);
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
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("HIGHRES_IMU", 1.0f);
configure_stream("ATTITUDE", 10.0f);
configure_stream("VFR_HUD", 8.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("RC_CHANNELS_RAW", 1.0f);
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[])
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
float base_rate_mult = _datarate / 1000.0f;
MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
_main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[])
}
if (fast_rate_limiter.check(t)) {
_mission_manager->eventloop();
/* only send messages if they fit the buffer */
if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
mavlink_pm_queued_send();
}
if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
_mission_manager->eventloop();
}
if (!mavlink_logbuffer_is_empty(&_logbuffer) &&
check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) {
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[])
}
delete _mission_manager;
delete _parameters_manager;
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;

58
src/modules/mavlink/mavlink_main.h
View File

@ -58,7 +58,7 @@
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
class Mavlink
{
@ -160,11 +160,7 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
bool check_can_send(const int prio, const unsigned packet_len);
void send_message(const uint8_t msgid, const void *msg, const int prio);
void send_message(const mavlink_message_t *msg);
void send_message(const uint8_t msgid, const void *msg);
void handle_message(const mavlink_message_t *msg);
@ -285,6 +281,7 @@ private:
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
orb_advert_t _mission_pub;
int _mission_result_sub;
@ -305,6 +302,7 @@ private:
int _baudrate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
@ -352,51 +350,12 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/**
* Send one parameter.
*
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
@ -420,6 +379,11 @@ private:
void pass_message(const mavlink_message_t *msg);
/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
void update_rate_mult();
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**

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

File diff suppressed because it is too large Load Diff

4
src/modules/mavlink/mavlink_messages.h
View File

@ -46,10 +46,10 @@
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
MavlinkStream* (*new_instance)(Mavlink *mavlink);
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
new_instance(inst),
get_name(name) {};

26
src/modules/mavlink/mavlink_mission.cpp
View File

@ -157,15 +157,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
@ -175,13 +173,11 @@ void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
if (seq < _count) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
} else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */
@ -199,15 +195,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
{
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _count;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
@ -226,13 +220,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mavlink_mission_item_t wp;
format_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
@ -251,13 +244,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (seq < _max_count) {
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
@ -272,13 +264,11 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
void
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
}

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

@ -38,11 +38,11 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
//#include <stdlib.h>
#include <stdio.h>
#include "mavlink_parameters.h"
#include "mavlink_main.h"
explicit
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1)
{
@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt
unsigned
MavlinkParametersManager::get_size()
{
if (_send_all_index() >= 0) {
if (_send_all_index >= 0) {
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {

3
src/modules/mavlink/mavlink_parameters.h
View File

@ -40,6 +40,9 @@
#pragma once
#include <systemlib/param/param.h>
#include "mavlink_bridge_header.h"
#include "mavlink_stream.h"
class MavlinkParametersManager : public MavlinkStream

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

@ -40,6 +40,7 @@ SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
mavlink_parameters.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \