mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-12 10:42:47 +08:00
MAVLink: Code style
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -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
@@ -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;
|
||||
|
||||
@@ -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")) {
|
||||
|
||||
@@ -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
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 &);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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(¶m_value.param_value, &hash, sizeof(hash));
|
||||
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_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, ¶m_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.");
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
/**
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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[]);
|
||||
|
||||
Reference in New Issue
Block a user