MAVLink: Code style

This commit is contained in:
Lorenz Meier
2016-08-26 23:43:33 +02:00
parent e67a6bdfc2
commit 720c445f21
17 changed files with 922 additions and 627 deletions
File diff suppressed because it is too large Load Diff
+30 -33
View File
@@ -35,7 +35,7 @@
/// @file mavlink_ftp.h
/// @author px4dev, Don Gagne <don@thegagnes.com>
#include <dirent.h>
#include <queue.h>
@@ -55,12 +55,12 @@ public:
~MavlinkFTP();
static MavlinkStream *new_instance(Mavlink *mavlink);
/// Handle possible FTP message
void handle_message(const mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
/// @brief Sets up the server to run in unit test mode.
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
/// @param worker_data Data to pass to worker
@@ -68,8 +68,7 @@ public:
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
/// 32 bit alignment to avoid usage of any pack pragmas.
struct PayloadHeader
{
struct PayloadHeader {
uint16_t seq_number; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
@@ -79,11 +78,10 @@ public:
uint8_t padding; ///< 32 bit aligment padding
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[]; ///< command data, varies by Opcode
};
};
/// @brief Command opcodes
enum Opcode : uint8_t
{
enum Opcode : uint8_t {
kCmdNone, ///< ignored, always acked
kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions
@@ -100,14 +98,13 @@ public:
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kCmdBurstReadFile, ///< Burst download session file
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
};
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
enum ErrorCode : uint8_t
{
enum ErrorCode : uint8_t {
kErrNone,
kErrFail, ///< Unknown failure
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
@@ -116,48 +113,48 @@ public:
kErrNoSessionsAvailable, ///< All available Sessions in use
kErrEOF, ///< Offset past end of file for List and Read commands
kErrUnknownCommand ///< Unknown command opcode
};
};
// MavlinkStream overrides
virtual const char *get_name(void) const;
virtual uint8_t get_id(void);
virtual unsigned get_size(void);
private:
char *_data_as_cstring(PayloadHeader* payload);
void _process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t* ftp_req);
char *_data_as_cstring(PayloadHeader *payload);
void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id);
void _reply(mavlink_file_transfer_protocol_t *ftp_req);
int _copy_file(const char *src_path, const char *dst_path, size_t length);
ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id);
ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
ErrorCode _workReset(PayloadHeader* payload);
ErrorCode _workReset(PayloadHeader *payload);
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload);
ErrorCode _workTruncateFile(PayloadHeader *payload);
ErrorCode _workRename(PayloadHeader *payload);
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
uint8_t _getServerSystemId(void);
uint8_t _getServerComponentId(void);
uint8_t _getServerChannel(void);
// Overrides from MavlinkStream
virtual void send(const hrt_abstime t);
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
/// @brief Maximum data size in RequestHeader::data
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
struct SessionInfo {
int fd;
uint32_t file_size;
@@ -168,15 +165,15 @@ private:
unsigned stream_chunk_transmitted;
};
struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
void *_worker_data; ///< Additional parameter to _utRcvMsgFunc;
/* do not allow copying this class */
MavlinkFTP(const MavlinkFTP&);
MavlinkFTP operator=(const MavlinkFTP&);
MavlinkFTP(const MavlinkFTP &);
MavlinkFTP operator=(const MavlinkFTP &);
// Mavlink test needs to be able to call send
friend class MavlinkFtpTest;
};
File diff suppressed because it is too large Load Diff
+19 -19
View File
@@ -53,13 +53,13 @@ public:
~LogListHelper();
public:
static void delete_all(const char* dir);
static void delete_all(const char *dir);
public:
bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0);
bool get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0);
bool open_for_transmit();
size_t get_log_data (uint8_t len, uint8_t* buffer);
size_t get_log_data(uint8_t len, uint8_t *buffer);
enum {
LOG_HANDLER_IDLE,
@@ -76,14 +76,14 @@ public:
uint32_t current_log_size;
uint32_t current_log_data_offset;
uint32_t current_log_data_remaining;
FILE* current_log_filep;
FILE *current_log_filep;
char current_log_filename[128];
private:
void _init ();
bool _get_session_date (const char* path, const char* dir, time_t& date);
void _scan_logs (FILE* f, const char* dir, time_t& date);
bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size);
void _init();
bool _get_session_date(const char *path, const char *dir, time_t &date);
void _scan_logs(FILE *f, const char *dir, time_t &date);
bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size);
};
// MAVLink LOG_* Message Handler
@@ -95,23 +95,23 @@ public:
static MavlinkLogHandler *new_instance(Mavlink *mavlink);
// Handle possible LOG message
void handle_message (const mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
// Overrides from MavlinkStream
const char* get_name (void) const;
uint8_t get_id (void);
unsigned get_size (void);
void send (const hrt_abstime t);
const char *get_name(void) const;
uint8_t get_id(void);
unsigned get_size(void);
void send(const hrt_abstime t);
private:
void _log_message (const mavlink_message_t *msg);
void _log_request_list (const mavlink_message_t *msg);
void _log_request_data (const mavlink_message_t *msg);
void _log_request_erase (const mavlink_message_t *msg);
void _log_request_end (const mavlink_message_t *msg);
void _log_message(const mavlink_message_t *msg);
void _log_request_list(const mavlink_message_t *msg);
void _log_request_data(const mavlink_message_t *msg);
void _log_request_erase(const mavlink_message_t *msg);
void _log_request_end(const mavlink_message_t *msg);
size_t _log_send_listing();
size_t _log_send_data ();
size_t _log_send_data();
private:
LogListHelper *_pLogHandlerHelper;
+94 -37
View File
@@ -117,7 +117,8 @@ extern mavlink_system_t mavlink_system;
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
m->send_bytes(ch, length);
}
@@ -125,7 +126,8 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
void mavlink_end_uart_send(mavlink_channel_t chan, int length)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
Mavlink *m = Mavlink::get_instance((unsigned)chan);
if (m != nullptr) {
(void)m->send_packet();
}
@@ -136,9 +138,11 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
*/
mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{
Mavlink* m = Mavlink::get_instance((unsigned)channel);
Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) {
return m->get_status();
} else {
return nullptr;
}
@@ -149,9 +153,11 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
*/
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{
Mavlink* m = Mavlink::get_instance((unsigned)channel);
Mavlink *m = Mavlink::get_instance((unsigned)channel);
if (m != nullptr) {
return m->get_buffer();
} else {
return nullptr;
}
@@ -219,7 +225,7 @@ Mavlink::Mavlink() :
_rate_txerr(0.0f),
_rate_rx(0.0f),
#ifdef __PX4_POSIX
_myaddr{},
_myaddr {},
_src_addr{},
_bcast_addr{},
_src_addr_initialized(false),
@@ -333,6 +339,7 @@ Mavlink::set_proto_version(unsigned version)
if (version == 1 || ((version == 0) && !_received_messages)) {
get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} else if (version == 2) {
get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
}
@@ -440,7 +447,7 @@ Mavlink::destroy_all_instances()
while (_mavlink_instances) {
inst_to_del = _mavlink_instances;
LL_DELETE(_mavlink_instances, inst_to_del);
delete (inst_to_del);
delete(inst_to_del);
}
printf("\n");
@@ -621,11 +628,11 @@ int Mavlink::get_component_id()
int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
{
#ifndef B460800
#define B460800 460800
#define B460800 460800
#endif
#ifndef B921600
#define B921600 921600
#define B921600 921600
#endif
/* process baud rate */
@@ -682,7 +689,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
/* back off 1800 ms to avoid running into the USB setup timing */
while (_mode == MAVLINK_MODE_CONFIG &&
hrt_absolute_time() < 1800U * 1000U) {
hrt_absolute_time() < 1800U * 1000U) {
usleep(50000);
}
@@ -856,15 +863,16 @@ Mavlink::get_free_tx_buf()
int buf_free = 0;
// if we are using network sockets, return max length of one packet
if (get_protocol() == UDP || get_protocol() == TCP ) {
if (get_protocol() == UDP || get_protocol() == TCP) {
return 1500;
} else {
// No FIONWRITE on Linux
#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
#else
//Linux cp210x does not support TIOCOUTQ
buf_free = 256;
//Linux cp210x does not support TIOCOUTQ
buf_free = 256;
#endif
if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
@@ -907,7 +915,7 @@ Mavlink::send_packet()
/* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!get_client_source_initialized()
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
if (!_broadcast_address_found) {
find_broadcast_address();
@@ -923,6 +931,7 @@ Mavlink::send_packet()
PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
_broadcast_failed_warned = true;
}
} else {
_broadcast_failed_warned = false;
}
@@ -960,8 +969,9 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
if (get_protocol() == SERIAL) {
/* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) {
/* no enough space in buffer to send */
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
@@ -986,6 +996,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
ret = packet_len;
}
}
#endif
if (ret != (size_t) packet_len) {
@@ -1018,16 +1029,19 @@ Mavlink::find_broadcast_address()
ifconf.ifc_len = 0;
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) {
PX4_WARN("getting required buffer size failed");
return;
}
#endif
PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len);
// Allocate buffer.
ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]);
if (ifconf.ifc_req == nullptr) {
PX4_ERR("Could not allocate ifconf buffer");
return;
@@ -1036,6 +1050,7 @@ Mavlink::find_broadcast_address()
memset(ifconf.ifc_req, 0, ifconf.ifc_len);
ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
if (ret != 0) {
PX4_ERR("getting network config failed");
delete[] ifconf.ifc_req;
@@ -1044,7 +1059,7 @@ Mavlink::find_broadcast_address()
size_t offset = 0;
// Later used to point to next network interface in buffer.
struct ifreq *cur_ifreq = (struct ifreq *)&(((uint8_t *)ifconf.ifc_req)[offset]);
struct ifreq *cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
// The ugly `for` construct is used because it allows to use
// `continue` and `break`.
@@ -1055,17 +1070,17 @@ Mavlink::find_broadcast_address()
// the interface name size plus whatever is greater, either the
// sizeof sockaddr or ifr_addr.sa_len.
offset += IF_NAMESIZE
+ (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ?
sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len)
+ (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ?
sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len)
#else
// On Linux, it's much easier to traverse the buffer, every entry
// has the constant length.
offset += sizeof(struct ifreq)
// On Linux, it's much easier to traverse the buffer, every entry
// has the constant length.
offset += sizeof(struct ifreq)
#endif
) {
// Point to next network interface in buffer.
cur_ifreq = (struct ifreq *)&(((uint8_t *)ifconf.ifc_req)[offset]);
cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
PX4_DEBUG("looking at %s", cur_ifreq->ifr_name);
@@ -1079,9 +1094,13 @@ Mavlink::find_broadcast_address()
}
struct ifreq bc_ifreq;
memset(&bc_ifreq, 0, sizeof(bc_ifreq));
strncpy(bc_ifreq.ifr_name, cur_ifreq->ifr_name, IF_NAMESIZE);
ret = ioctl(_socket_fd, SIOCGIFBRDADDR, &bc_ifreq);
if (ret != 0) {
PX4_DEBUG("getting broadcast address failed for %s", cur_ifreq->ifr_name);
continue;
@@ -1109,6 +1128,7 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_addr = bc_addr;
_broadcast_address_found = true;
} else {
PX4_INFO("ignoring additional network interface %s, IP: %s",
cur_ifreq->ifr_name, inet_ntoa(sin_addr));
@@ -1119,9 +1139,11 @@ Mavlink::find_broadcast_address()
_bcast_addr.sin_port = htons(_remote_port);
int broadcast_opt = 1;
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
PX4_WARN("setting broadcast permission failed");
}
_broadcast_address_not_found_warned = false;
} else {
@@ -1162,6 +1184,7 @@ Mavlink::init_udp()
_src_addr.sin_family = AF_INET;
inet_aton("127.0.0.1", &_src_addr.sin_addr);
}
_src_addr.sin_port = htons(_remote_port);
#endif
@@ -1515,15 +1538,18 @@ Mavlink::get_rate_mult()
return _rate_mult;
}
MavlinkShell*
MavlinkShell *
Mavlink::get_shell()
{
if (!_mavlink_shell) {
_mavlink_shell = new MavlinkShell();
if (!_mavlink_shell) {
PX4_ERR("Failed to allocate a shell");
} else {
int ret = _mavlink_shell->start();
if (ret != 0) {
PX4_ERR("Failed to start shell (%i)", ret);
delete _mavlink_shell;
@@ -1590,14 +1616,17 @@ Mavlink::update_rate_mult()
/* scale down if we have a TX err rate suggesting link congestion */
if (_rate_txerr > 0.0f && !radio_critical) {
hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
} else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
hardware_mult *= 0.80f;
} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
hardware_mult *= 0.975f;
} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
hardware_mult *= 1.025f;
@@ -1644,7 +1673,7 @@ Mavlink::task_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
#ifdef __PX4_POSIX
char* eptr;
char *eptr;
int temp_int_arg;
#endif
@@ -1676,38 +1705,49 @@ Mavlink::task_main(int argc, char *argv[])
break;
#ifdef __PX4_POSIX
case 'u':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_network_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid data udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 'o':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
_remote_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid remote udp_port '%s'", myoptarg);
err_flag = true;
}
break;
case 't':
_src_addr.sin_family = AF_INET;
if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
_src_addr_initialized = true;
} else {
warnx("invalid partner ip '%s'", myoptarg);
err_flag = true;
}
break;
#else
case 'u':
case 'o':
case 't':
@@ -1797,6 +1837,7 @@ Mavlink::task_main(int argc, char *argv[])
if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
warn("could not open %s", _device_name);
return ERROR;
} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
/* the config link is optional */
return OK;
@@ -1991,6 +2032,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("MISSION_ITEM", 50.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
default:
break;
}
@@ -2053,6 +2095,7 @@ Mavlink::task_main(int argc, char *argv[])
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);
} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
@@ -2073,6 +2116,7 @@ Mavlink::task_main(int argc, char *argv[])
// config thing, we leave the file struct
// allocated.
//fclose(fs);
} else {
PX4_WARN("open fd %d failed", _uart_fd);
}
@@ -2099,6 +2143,7 @@ Mavlink::task_main(int argc, char *argv[])
}
struct mavlink_log_s mavlink_log;
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
_logbuffer.put(&mavlink_log);
}
@@ -2118,26 +2163,29 @@ Mavlink::task_main(int argc, char *argv[])
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
if ( get_protocol() == SERIAL ) {
if (get_protocol() == SERIAL) {
PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
(double)_subscribe_to_stream_rate);
}
} else {
if ( get_protocol() == SERIAL ) {
if (get_protocol() == SERIAL) {
PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
}
}
} else {
if ( get_protocol() == SERIAL ) {
if (get_protocol() == SERIAL) {
PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
} else if ( get_protocol() == UDP ) {
} else if (get_protocol() == UDP) {
PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
}
}
@@ -2302,7 +2350,7 @@ Mavlink::start(int argc, char *argv[])
if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
warnx("Maximum MAVLink instance count of %d reached.",
(int)Mavlink::MAVLINK_MAX_INSTANCES);
(int)Mavlink::MAVLINK_MAX_INSTANCES);
return 1;
}
@@ -2399,7 +2447,7 @@ Mavlink::stream_command(int argc, char *argv[])
float rate = -1.0f;
const char *stream_name = nullptr;
unsigned short network_port = 0;
char* eptr;
char *eptr;
int temp_int_arg;
bool provided_device = false;
bool provided_network_port = false;
@@ -2438,15 +2486,20 @@ Mavlink::stream_command(int argc, char *argv[])
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
stream_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if ( *eptr == '\0' ) {
if (*eptr == '\0') {
network_port = temp_int_arg;
} else {
err_flag = true;
}
i++;
} else {
err_flag = true;
}
@@ -2457,10 +2510,13 @@ Mavlink::stream_command(int argc, char *argv[])
if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = nullptr;
if (provided_device && !provided_network_port) {
inst = get_instance_for_device(device_name);
} else if (provided_network_port && !provided_device) {
inst = get_instance_for_network_port(network_port);
} else if (provided_device && provided_network_port) {
warnx("please provide either a device name or a network port");
return 1;
@@ -2475,6 +2531,7 @@ Mavlink::stream_command(int argc, char *argv[])
// because this is so easy to get wrong and not fatal. Warning is sufficient.
if (provided_device) {
warnx("mavlink for device %s is not running", device_name);
} else {
warnx("mavlink for network on port %hu is not running", network_port);
}
@@ -2499,8 +2556,8 @@ Mavlink::set_boot_complete()
Mavlink *inst;
LL_FOREACH(::_mavlink_instances, inst) {
if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
(!inst->broadcast_enabled()) &&
((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
(!inst->broadcast_enabled()) &&
((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
}
}
@@ -2543,7 +2600,7 @@ int mavlink_main(int argc, char *argv[])
usage();
return 1;
} else if (!strcmp(argv[1], "stop-all") ) {
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
} else if (!strcmp(argv[1], "status")) {
+32 -24
View File
@@ -170,22 +170,29 @@ public:
BROADCAST_MODE_ON
};
static const char *mavlink_mode_str(enum MAVLINK_MODE mode) {
static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
{
switch (mode) {
case MAVLINK_MODE_NORMAL:
return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom";
case MAVLINK_MODE_ONBOARD:
return "Onboard";
case MAVLINK_MODE_OSD:
return "OSD";
case MAVLINK_MODE_MAGIC:
return "Magic";
case MAVLINK_MODE_CONFIG:
return "Config";
default:
return "Unknown";
case MAVLINK_MODE_NORMAL:
return "Normal";
case MAVLINK_MODE_CUSTOM:
return "Custom";
case MAVLINK_MODE_ONBOARD:
return "Onboard";
case MAVLINK_MODE_OSD:
return "OSD";
case MAVLINK_MODE_MAGIC:
return "Magic";
case MAVLINK_MODE_CONFIG:
return "Config";
default:
return "Unknown";
}
}
@@ -230,7 +237,8 @@ public:
*/
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
/**
* Enable / disable Hardware in the Loop simulation mode.
@@ -285,7 +293,7 @@ public:
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0);
int get_instance_id();
@@ -339,7 +347,7 @@ public:
void send_statustext(unsigned char severity, const char *string);
void send_autopilot_capabilites();
MavlinkStream * get_streams() const { return _streams; }
MavlinkStream *get_streams() const { return _streams; }
float get_rate_mult();
@@ -380,7 +388,7 @@ public:
/**
* Get the receive status of this MAVLink link
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
struct telemetry_status_s &get_rx_status() { return _rstatus; }
ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; }
@@ -392,9 +400,9 @@ public:
unsigned short get_remote_port() { return _remote_port; }
int get_socket_fd () { return _socket_fd; };
int get_socket_fd() { return _socket_fd; };
#ifdef __PX4_POSIX
struct sockaddr_in * get_client_source_address() { return &_src_addr; }
struct sockaddr_in *get_client_source_address() { return &_src_addr; }
void set_client_source_initialized() { _src_addr_initialized = true; }
@@ -419,7 +427,7 @@ public:
void set_logging_enabled(bool logging) { _logging_enabled = logging; }
int get_data_rate() { return _datarate; }
void set_data_rate(int rate) { if (rate > 0) _datarate = rate; }
void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
* Returns nullptr if shell cannot be created */
@@ -608,6 +616,6 @@ private:
int task_main(int argc, char *argv[]);
/* do not allow copying this class */
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
Mavlink(const Mavlink &);
Mavlink operator=(const Mavlink &);
};
File diff suppressed because it is too large Load Diff
+26 -12
View File
@@ -66,9 +66,9 @@ int MavlinkMissionManager::_current_seq = 0;
bool MavlinkMissionManager::_transfer_in_progress = false;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@@ -120,9 +120,11 @@ void
MavlinkMissionManager::init_offboard_mission()
{
mission_s mission_state;
if (!_dataman_init) {
_dataman_init = true;
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s);
if (ret > 0) {
_dataman_id = mission_state.dataman_id;
_count = mission_state.count;
@@ -132,6 +134,7 @@ MavlinkMissionManager::init_offboard_mission()
_dataman_id = 0;
_count = 0;
_current_seq = 0;
} else {
PX4_WARN("offboard mission init failed");
_dataman_id = 0;
@@ -139,6 +142,7 @@ MavlinkMissionManager::init_offboard_mission()
_current_seq = 0;
}
}
_my_dataman_id = _dataman_id;
}
@@ -176,6 +180,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
} else {
warnx("WPM: ERROR: can't save mission state");
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
}
@@ -261,6 +266,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to read from microSD");
}
@@ -588,11 +594,11 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
if(_transfer_in_progress)
{
if (_transfer_in_progress) {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
return;
}
_transfer_in_progress = true;
if (wpc.count > _max_count) {
@@ -683,6 +689,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
}
struct mission_item_s mission_item = {};
int ret = parse_mavlink_mission_item(&wp, &mission_item);
if (ret != OK) {
@@ -698,7 +705,8 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item,
sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -729,6 +737,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
}
_transfer_in_progress = false;
} else {
@@ -753,6 +762,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else {
@@ -768,10 +778,11 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
}
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
struct mission_item_s *mission_item)
{
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
// only support global waypoints for now
@@ -781,6 +792,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
mission_item->altitude_is_relative = false;
} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
mission_item->altitude_is_relative = true;
}
@@ -897,7 +909,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item)
{
mavlink_mission_item->frame = mission_item->frame;
mavlink_mission_item->command = mission_item->nav_cmd;
@@ -943,6 +956,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
}
@@ -997,10 +1011,10 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
void MavlinkMissionManager::check_active_mission(void)
{
if(!(_my_dataman_id==_dataman_id))
{
if (!(_my_dataman_id == _dataman_id)) {
if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
_my_dataman_id=_dataman_id;
_my_dataman_id = _dataman_id;
this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);
}
}
+8 -5
View File
@@ -67,7 +67,8 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class MavlinkMissionManager : public MavlinkStream {
class MavlinkMissionManager : public MavlinkStream
{
public:
~MavlinkMissionManager();
@@ -113,7 +114,7 @@ private:
static int _dataman_id; ///< Global Dataman storage ID for active mission
int _my_dataman_id; ///< class Dataman storage ID
static bool _dataman_init; ///< Dataman initialized
static unsigned _count; ///< Count of items in active mission
static int _current_seq; ///< Current item sequence in active mission
@@ -133,11 +134,12 @@ private:
bool _verbose;
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
2; ///< Error count limit before stopping to report FS errors
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
MavlinkMissionManager &operator = (const MavlinkMissionManager &);
void init_offboard_mission();
@@ -196,7 +198,8 @@ private:
/**
* Format mission_item_s as mavlink MISSION_ITEM message.
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
int format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item);
protected:
explicit MavlinkMissionManager(Mavlink *mavlink);
@@ -80,6 +80,7 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
// if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
@@ -90,11 +91,13 @@ MavlinkOrbSubscription::update(uint64_t *time, void *data)
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
} else {
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
@@ -115,6 +118,7 @@ bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
bool updated;
if (orb_check(_fd, &updated)) {
return false;
}
@@ -128,6 +132,7 @@ MavlinkOrbSubscription::update_if_changed(void *data)
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
}
@@ -97,8 +97,8 @@ private:
bool _published; ///< topic was ever published
/* do not allow copying this class */
MavlinkOrbSubscription(const MavlinkOrbSubscription&);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&);
MavlinkOrbSubscription(const MavlinkOrbSubscription &);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription &);
};
+27 -2
View File
@@ -62,6 +62,7 @@ MavlinkParametersManager::~MavlinkParametersManager()
if (_uavcan_parameter_value_sub >= 0) {
orb_unsubscribe(_uavcan_parameter_value_sub);
}
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
@@ -92,6 +93,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
if (_send_all_index < 0) {
_send_all_index = PARAM_HASH;
} else {
/* a restart should skip the hash check on the ground */
_send_all_index = 0;
@@ -108,10 +110,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -160,9 +164,11 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.param_index = -1;
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value;
} else {
int32_t val;
memcpy(&val, &set.param_value, sizeof(int32_t));
@@ -172,10 +178,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -202,6 +210,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
} else {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -211,6 +220,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */
int ret = send_param(param_for_used_index(req_read.param_index));
@@ -219,6 +229,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
} else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
@@ -228,7 +239,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
@@ -239,10 +250,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break;
}
@@ -258,18 +271,22 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id,
MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
_rc_param_map.value_max[i] = map_rc.param_value_max;
if (map_rc.param_index == -2) { // -2 means unset map
_rc_param_map.valid[i] = false;
} else {
_rc_param_map.valid[i] = true;
}
_rc_param_map.timestamp = hrt_absolute_time();
if (_rc_param_map_pub == nullptr) {
@@ -280,6 +297,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
break;
}
@@ -300,6 +318,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (space_available && param_value_ready) {
struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
@@ -308,16 +327,20 @@ MavlinkParametersManager::send(const hrt_abstime t)
msg.param_count = value.param_count;
msg.param_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val;
val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */
@@ -351,6 +374,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
@@ -364,6 +388,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
} else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) {
/* the boot did not seem to ever complete, warn user and set boot complete */
_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
+1 -1
View File
@@ -82,7 +82,7 @@ private:
/* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &);
MavlinkParametersManager &operator = (const MavlinkParametersManager &);
protected:
explicit MavlinkParametersManager(Mavlink *mavlink);
+3 -2
View File
@@ -1201,11 +1201,12 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
// we only support shell commands
if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
|| (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
|| (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
return;
}
MavlinkShell* shell = _mavlink->get_shell();
MavlinkShell *shell = _mavlink->get_shell();
if (shell) {
// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
if (serial_control_mavlink.count > 0) {
+1 -1
View File
@@ -155,7 +155,7 @@ private:
* @param interval the interval in us to send the message at
* @param data_rate the total link data rate in bytes per second
*/
void set_message_interval(int msgId, float interval, int data_rate=-1);
void set_message_interval(int msgId, float interval, int data_rate = -1);
void get_message_interval(int msgId);
/**
+16 -7
View File
@@ -62,6 +62,7 @@ MavlinkShell::~MavlinkShell()
PX4_INFO("Stopping mavlink shell");
close(_to_shell_fd);
}
if (_from_shell_fd >= 0) {
close(_from_shell_fd);
}
@@ -91,6 +92,7 @@ int MavlinkShell::start()
if (pipe(p1) != 0) {
return -errno;
}
if (pipe(p2) != 0) {
close(p1[0]);
close(p1[1]);
@@ -105,22 +107,26 @@ int MavlinkShell::start()
_shell_fds[1] = p1[1];
int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task
for (int i = 0; i < 2; ++i) {
fd_backups[i] = dup(i);
if (fd_backups[i] == -1) {
ret = -errno;
}
}
dup2(_shell_fds[0], 0);
dup2(_shell_fds[1], 1);
if (ret == 0) {
_task = px4_task_spawn_cmd("mavlink_shell",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
&MavlinkShell::shell_start_thread,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
&MavlinkShell::shell_start_thread,
nullptr);
if (_task < 0) {
ret = -1;
}
@@ -131,6 +137,7 @@ int MavlinkShell::start()
if (dup2(fd_backups[i], i) == -1) {
ret = -errno;
}
close(fd_backups[i]);
}
@@ -152,12 +159,12 @@ int MavlinkShell::shell_start_thread(int argc, char *argv[])
return 0;
}
size_t MavlinkShell::write(uint8_t* buffer, size_t len)
size_t MavlinkShell::write(uint8_t *buffer, size_t len)
{
return ::write(_to_shell_fd, buffer, len);
}
size_t MavlinkShell::read(uint8_t* buffer, size_t len)
size_t MavlinkShell::read(uint8_t *buffer, size_t len)
{
return ::read(_from_shell_fd, buffer, len);
}
@@ -165,8 +172,10 @@ size_t MavlinkShell::read(uint8_t* buffer, size_t len)
size_t MavlinkShell::available()
{
int ret = 0;
if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
return ret;
}
return 0;
}
+3 -3
View File
@@ -64,14 +64,14 @@ public:
* Write to the shell
* @return number of written bytes
*/
size_t write(uint8_t* buffer, size_t len);
size_t write(uint8_t *buffer, size_t len);
/**
* Read from the shell. This is blocking, if 0 bytes are available, this will block.
* @param len buffer length
* @return number of bytes read.
*/
size_t read(uint8_t* buffer, size_t len);
size_t read(uint8_t *buffer, size_t len);
/**
* Get the number of bytes that can be read.
@@ -82,7 +82,7 @@ private:
int _to_shell_fd = -1; /** fd to write to the shell */
int _from_shell_fd = -1; /** fd to read from the shell */
int _shell_fds[2] = {-1, -1}; /** stdin & out used by the shell */
int _shell_fds[2] = { -1, -1}; /** stdin & out used by the shell */
px4_task_t _task;
static int shell_start_thread(int argc, char *argv[]);