fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: use reference instead of pointer to access the MAVLink instance from protocol classes

Browse Source
This commit is contained in:
Matthias Grob
2024-06-05 16:13:52 +02:00
parent 9a7a977625
commit 68769ea0ec
16 changed files with 156 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ftp.cpp
+8 -12
View File
@@ -45,17 +45,13 @@
#include "mavlink_ftp.h" #include "mavlink_ftp.h"
#include "mavlink_tests/mavlink_ftp_test.h" #include "mavlink_tests/mavlink_ftp_test.h"
#ifndef MAVLINK_FTP_UNIT_TEST
#include "mavlink_main.h" #include "mavlink_main.h"
#else
#include <mavlink.h>
#endif
using namespace time_literals; using namespace time_literals;
constexpr const char MavlinkFTP::_root_dir[]; constexpr const char MavlinkFTP::_root_dir[];
MavlinkFTP::MavlinkFTP(Mavlink *mavlink) : MavlinkFTP::MavlinkFTP(Mavlink &mavlink) :
_mavlink(mavlink) _mavlink(mavlink)
{ {
// initialize session // initialize session
@@ -96,7 +92,7 @@ MavlinkFTP::_getServerSystemId()
return MavlinkFtpTest::serverSystemId; return MavlinkFtpTest::serverSystemId;
#else #else
// Not unit testing, use the real thing // Not unit testing, use the real thing
return _mavlink->get_system_id(); return _mavlink.get_system_id();
#endif #endif
} }
@@ -108,7 +104,7 @@ MavlinkFTP::_getServerComponentId()
return MavlinkFtpTest::serverComponentId; return MavlinkFtpTest::serverComponentId;
#else #else
// Not unit testing, use the real thing // Not unit testing, use the real thing
return _mavlink->get_component_id(); return _mavlink.get_component_id();
#endif #endif
} }
@@ -120,7 +116,7 @@ MavlinkFTP::_getServerChannel()
return MavlinkFtpTest::serverChannel; return MavlinkFtpTest::serverChannel;
#else #else
// Not unit testing, use the real thing // Not unit testing, use the real thing
return _mavlink->get_channel(); return _mavlink.get_channel();
#endif #endif
} }
@@ -180,7 +176,7 @@ MavlinkFTP::_process_request(
#ifdef MAVLINK_FTP_UNIT_TEST #ifdef MAVLINK_FTP_UNIT_TEST
_utRcvMsgFunc(last_reply, _worker_data); _utRcvMsgFunc(last_reply, _worker_data);
#else #else
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply); mavlink_msg_file_transfer_protocol_send_struct(_mavlink.get_channel(), last_reply);
#endif #endif
return; return;
} }
@@ -340,7 +336,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req)
// Unit test hook is set, call that instead // Unit test hook is set, call that instead
_utRcvMsgFunc(ftp_req, _worker_data); _utRcvMsgFunc(ftp_req, _worker_data);
#else #else
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req); mavlink_msg_file_transfer_protocol_send_struct(_mavlink.get_channel(), ftp_req);
#endif #endif
} }
@@ -1055,8 +1051,8 @@ void MavlinkFTP::send()
#ifndef MAVLINK_FTP_UNIT_TEST #ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room // Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf(); unsigned max_bytes_to_send = _mavlink.get_free_tx_buf();
PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf()); PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink.get_free_tx_buf());
if (max_bytes_to_send < get_size()) { if (max_bytes_to_send < get_size()) {
return; return;
src/modules/mavlink/mavlink_ftp.h
+2 -6
View File
@@ -43,11 +43,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#ifndef MAVLINK_FTP_UNIT_TEST
#include "mavlink_bridge_header.h" #include "mavlink_bridge_header.h"
#else
#include <mavlink.h>
#endif
class MavlinkFtpTest; class MavlinkFtpTest;
class Mavlink; class Mavlink;
@@ -56,7 +52,7 @@ class Mavlink;
class MavlinkFTP class MavlinkFTP
{ {
public: public:
MavlinkFTP(Mavlink *mavlink); MavlinkFTP(Mavlink &mavlink);
~MavlinkFTP(); ~MavlinkFTP();
/** /**
@@ -190,7 +186,7 @@ private:
ReceiveMessageFunc_t _utRcvMsgFunc{}; ///< Unit test override for mavlink message sending ReceiveMessageFunc_t _utRcvMsgFunc{}; ///< Unit test override for mavlink message sending
void *_worker_data{nullptr}; ///< Additional parameter to _utRcvMsgFunc; void *_worker_data{nullptr}; ///< Additional parameter to _utRcvMsgFunc;
Mavlink *_mavlink; Mavlink &_mavlink;
/* do not allow copying this class */ /* do not allow copying this class */
MavlinkFTP(const MavlinkFTP &); MavlinkFTP(const MavlinkFTP &);
src/modules/mavlink/mavlink_log_handler.cpp
+5 -5
View File
@@ -80,7 +80,7 @@ stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr)
} }
//------------------------------------------------------------------- //-------------------------------------------------------------------
MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink)
: _mavlink(mavlink) : _mavlink(mavlink)
{ {
@@ -123,14 +123,14 @@ MavlinkLogHandler::send()
//-- Log Entries //-- Log Entries
while (_current_status == LogHandlerState::Listing while (_current_status == LogHandlerState::Listing
&& _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES
&& count < MAX_BYTES_SEND) { && count < MAX_BYTES_SEND) {
count += _log_send_listing(); count += _log_send_listing();
} }
//-- Log Data //-- Log Data
while (_current_status == LogHandlerState::SendingData while (_current_status == LogHandlerState::SendingData
&& _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES
&& count < MAX_BYTES_SEND) { && count < MAX_BYTES_SEND) {
count += _log_send_data(); count += _log_send_data();
} }
@@ -272,7 +272,7 @@ MavlinkLogHandler::_log_send_listing()
response.id = _next_entry; response.id = _next_entry;
response.num_logs = _log_count; response.num_logs = _log_count;
response.last_log_num = _last_entry; response.last_log_num = _last_entry;
mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &response);
//-- If we're done listing, flag it. //-- If we're done listing, flag it.
if (_next_entry >= _last_entry) { if (_next_entry >= _last_entry) {
@@ -309,7 +309,7 @@ MavlinkLogHandler::_log_send_data()
response.ofs = _current_log_data_offset; response.ofs = _current_log_data_offset;
response.id = _current_log_index; response.id = _current_log_index;
response.count = read_size; response.count = read_size;
mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response); mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &response);
_current_log_data_offset += read_size; _current_log_data_offset += read_size;
_current_log_data_remaining -= read_size; _current_log_data_remaining -= read_size;
src/modules/mavlink/mavlink_log_handler.h
+2 -2
View File
@@ -51,7 +51,7 @@ class Mavlink;
class MavlinkLogHandler class MavlinkLogHandler
{ {
public: public:
MavlinkLogHandler(Mavlink *mavlink); MavlinkLogHandler(Mavlink &mavlink);
~MavlinkLogHandler(); ~MavlinkLogHandler();
// Handle possible LOG message // Handle possible LOG message
@@ -93,7 +93,7 @@ private:
size_t _log_send_data(); size_t _log_send_data();
LogHandlerState _current_status{LogHandlerState::Inactive}; LogHandlerState _current_status{LogHandlerState::Inactive};
Mavlink *_mavlink; Mavlink &_mavlink;
int _next_entry{0}; int _next_entry{0};
int _last_entry{0}; int _last_entry{0};
src/modules/mavlink/mavlink_main.cpp
+3 -3
View File
@@ -100,7 +100,7 @@ bool Mavlink::_boot_complete = false;
Mavlink::Mavlink() : Mavlink::Mavlink() :
ModuleParams(nullptr), ModuleParams(nullptr),
_receiver(this) _receiver(*this)
{ {
// initialise parameter cache // initialise parameter cache
mavlink_update_parameters(); mavlink_update_parameters();
@@ -438,12 +438,12 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self)
} }
bool bool
Mavlink::component_was_seen(int system_id, int component_id, Mavlink *self) Mavlink::component_was_seen(int system_id, int component_id, Mavlink &self)
{ {
LockGuard lg{mavlink_module_mutex}; LockGuard lg{mavlink_module_mutex};
for (Mavlink *inst : mavlink_module_instances) { for (Mavlink *inst : mavlink_module_instances) {
if (inst && (inst != self) && (inst->_receiver.component_was_seen(system_id, component_id))) { if (inst && (inst != &self) && (inst->_receiver.component_was_seen(system_id, component_id))) {
return true; return true;
} }
} }
src/modules/mavlink/mavlink_main.h
+1 -1
View File
@@ -172,7 +172,7 @@ public:
static bool serial_instance_exists(const char *device_name, Mavlink *self); static bool serial_instance_exists(const char *device_name, Mavlink *self);
static bool component_was_seen(int system_id, int component_id, Mavlink *self = nullptr); static bool component_was_seen(int system_id, int component_id, Mavlink &self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self);
src/modules/mavlink/mavlink_mission.cpp
+35 -35
View File
@@ -72,7 +72,7 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL))) (_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
_mavlink(mavlink) _mavlink(mavlink)
{ {
if (!_dataman_init) { if (!_dataman_init) {
@@ -209,7 +209,7 @@ MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigne
} else { } else {
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); _mavlink.send_statustext_critical("Mission storage: Unable to write to microSD\t");
events::send(events::ID("mavlink_mission_storage_write_failure2"), events::Log::Critical, events::send(events::ID("mavlink_mission_storage_write_failure2"), events::Log::Critical,
"Mission: Unable to write to storage"); "Mission: Unable to write to storage");
} }
@@ -245,7 +245,7 @@ MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, un
} else { } else {
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
_mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); _mavlink.send_statustext_critical("Mission storage: Unable to write to microSD\t");
events::send(events::ID("mavlink_mission_storage_write_failure3"), events::Log::Critical, events::send(events::ID("mavlink_mission_storage_write_failure3"), events::Log::Critical,
"Mission: Unable to write to storage"); "Mission: Unable to write to storage");
} }
@@ -270,7 +270,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
wpa.mission_type = _mission_type; wpa.mission_type = _mission_type;
wpa.opaque_id = opaque_id; wpa.opaque_id = opaque_id;
mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); mavlink_msg_mission_ack_send_struct(_mavlink.get_channel(), &wpa);
PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
} }
@@ -284,7 +284,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE]; wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE];
wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); mavlink_msg_mission_current_send_struct(_mavlink.get_channel(), &wpc);
PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq);
} }
@@ -303,7 +303,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
wpc.mission_type = mission_type; wpc.mission_type = mission_type;
wpc.opaque_id = opaque_id; wpc.opaque_id = opaque_id;
mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); mavlink_msg_mission_count_send_struct(_mavlink.get_channel(), &wpc);
PX4_DEBUG("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type); PX4_DEBUG("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type);
} }
@@ -350,7 +350,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
break; break;
default: default:
_mavlink->send_statustext_critical("Received unknown mission type, abort.\t"); _mavlink.send_statustext_critical("Received unknown mission type, abort.\t");
events::send(events::ID("mavlink_mission_recv_unknown_mis_type"), events::Log::Error, events::send(events::ID("mavlink_mission_recv_unknown_mis_type"), events::Log::Error,
"Received unknown mission type, abort"); "Received unknown mission type, abort");
break; break;
@@ -368,7 +368,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
wp.seq = seq; wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0; wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_int_send_struct(_mavlink->get_channel(), &wp); mavlink_msg_mission_item_int_send_struct(_mavlink.get_channel(), &wp);
PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system); PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
@@ -381,7 +381,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
wp.seq = seq; wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0; wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp); mavlink_msg_mission_item_send_struct(_mavlink.get_channel(), &wp);
PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system);
} }
@@ -390,7 +390,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
mavlink_log_critical(_mavlink->get_mavlink_log_pub(), mavlink_log_critical(_mavlink.get_mavlink_log_pub(),
"Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type); "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type);
/* EVENT /* EVENT
* @description Mission type: {1} * @description Mission type: {1}
@@ -448,7 +448,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
wpr.target_component = compid; wpr.target_component = compid;
wpr.seq = seq; wpr.seq = seq;
wpr.mission_type = _mission_type; wpr.mission_type = _mission_type;
mavlink_msg_mission_request_int_send_struct(_mavlink->get_channel(), &wpr); mavlink_msg_mission_request_int_send_struct(_mavlink.get_channel(), &wpr);
PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system); PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
@@ -460,13 +460,13 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
wpr.seq = seq; wpr.seq = seq;
wpr.mission_type = _mission_type; wpr.mission_type = _mission_type;
mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr); mavlink_msg_mission_request_send_struct(_mavlink.get_channel(), &wpr);
PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system);
} }
} else { } else {
_mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity\t"); _mavlink.send_statustext_critical("ERROR: Waypoint index exceeds list capacity\t");
events::send<uint16_t>(events::ID("mavlink_mission_wp_index_exceeds_list"), events::Log::Error, events::send<uint16_t>(events::ID("mavlink_mission_wp_index_exceeds_list"), events::Log::Error,
"Waypoint index eceeds list capacity (maximum: {1})", current_max_item_count()); "Waypoint index eceeds list capacity (maximum: {1})", current_max_item_count());
@@ -481,7 +481,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
wp_reached.seq = seq; wp_reached.seq = seq;
mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached); mavlink_msg_mission_item_reached_send_struct(_mavlink.get_channel(), &wp_reached);
PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq);
} }
@@ -490,7 +490,7 @@ void
MavlinkMissionManager::send() MavlinkMissionManager::send()
{ {
// do not send anything over high latency communication // do not send anything over high latency communication
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { if (_mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
return; return;
} }
@@ -509,7 +509,7 @@ MavlinkMissionManager::send()
send_mission_current(_current_seq); send_mission_current(_current_seq);
} else { } else {
_mavlink->send_statustext_critical("ERROR: wp index out of bounds\t"); _mavlink.send_statustext_critical("ERROR: wp index out of bounds\t");
events::send<uint16_t, uint16_t>(events::ID("mavlink_mission_wp_index_out_of_bounds"), events::Log::Error, events::send<uint16_t, uint16_t>(events::ID("mavlink_mission_wp_index_out_of_bounds"), events::Log::Error,
"Waypoint index out of bounds (current {1} \\>= total {2})", mission_result.seq_current, mission_result.seq_total); "Waypoint index out of bounds (current {1} \\>= total {2})", mission_result.seq_current, mission_result.seq_total);
} }
@@ -562,7 +562,7 @@ MavlinkMissionManager::send()
} else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
&& hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) {
_mavlink->send_statustext_critical("Operation timeout\t"); _mavlink.send_statustext_critical("Operation timeout\t");
events::send(events::ID("mavlink_mission_op_timeout"), events::Log::Error, events::send(events::ID("mavlink_mission_op_timeout"), events::Log::Error,
"Operation timeout, aborting transfer"); "Operation timeout, aborting transfer");
@@ -674,7 +674,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
} }
} else { } else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch\t"); _mavlink.send_statustext_critical("REJ. WP CMD: partner id mismatch\t");
events::send(events::ID("mavlink_mission_partner_id_mismatch"), events::Log::Error, events::send(events::ID("mavlink_mission_partner_id_mismatch"), events::Log::Error,
"Rejecting waypoint command, component or system ID mismatch"); "Rejecting waypoint command, component or system ID mismatch");
@@ -699,7 +699,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
} else { } else {
PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq);
_mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list\t"); _mavlink.send_statustext_critical("WPM: WP CURR CMD: Not in list\t");
events::send(events::ID("mavlink_mission_seq_out_of_bounds"), events::Log::Error, events::send(events::ID("mavlink_mission_seq_out_of_bounds"), events::Log::Error,
"New mission waypoint sequence out of bounds"); "New mission waypoint sequence out of bounds");
} }
@@ -707,7 +707,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_SET_CURRENT ERROR: busy"); PX4_DEBUG("WPM: MISSION_SET_CURRENT ERROR: busy");
_mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy\t"); _mavlink.send_statustext_critical("WPM: IGN WP CURR CMD: Busy\t");
events::send(events::ID("mavlink_mission_state_busy"), events::Log::Error, events::send(events::ID("mavlink_mission_state_busy"), events::Log::Error,
"Mission manager currently busy, ignoring new waypoint index"); "Mission manager currently busy, ignoring new waypoint index");
} }
@@ -768,7 +768,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy"); PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy");
_mavlink->send_statustext_info("Mission download request ignored, already active\t"); _mavlink.send_statustext_info("Mission download request ignored, already active\t");
events::send(events::ID("mavlink_mission_req_ignored"), events::Log::Warning, events::send(events::ID("mavlink_mission_req_ignored"), events::Log::Warning,
"Mission download request ignored, already active"); "Mission download request ignored, already active");
} }
@@ -843,7 +843,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
switch_to_idle_state(); switch_to_idle_state();
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); _mavlink.send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t");
events::send(events::ID("mavlink_mission_wp_unexpected"), events::Log::Error, events::send(events::ID("mavlink_mission_wp_unexpected"), events::Log::Error,
"Unexpected waypoint index, aborting transfer"); "Unexpected waypoint index, aborting transfer");
return; return;
@@ -860,7 +860,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
switch_to_idle_state(); switch_to_idle_state();
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); _mavlink.send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t");
events::send(events::ID("mavlink_mission_wp_unexpected2"), events::Log::Error, events::send(events::ID("mavlink_mission_wp_unexpected2"), events::Log::Error,
"Unexpected waypoint index, aborting mission transfer"); "Unexpected waypoint index, aborting mission transfer");
} }
@@ -869,18 +869,18 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer"); PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer");
// Silently ignore this as some OSDs have buggy mission protocol implementations // Silently ignore this as some OSDs have buggy mission protocol implementations
//_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer"); //_mavlink.send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer");
} else { } else {
PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state); PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy\t"); _mavlink.send_statustext_critical("WPM: REJ. CMD: Busy\t");
events::send(events::ID("mavlink_mission_mis_req_ignored_busy"), events::Log::Error, events::send(events::ID("mavlink_mission_mis_req_ignored_busy"), events::Log::Error,
"Ignoring mission request, currently busy"); "Ignoring mission request, currently busy");
} }
} else { } else {
_mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch\t"); _mavlink.send_statustext_critical("WPM: REJ. CMD: partner id mismatch\t");
events::send(events::ID("mavlink_mission_partner_id_mismatch2"), events::Log::Error, events::send(events::ID("mavlink_mission_partner_id_mismatch2"), events::Log::Error,
"Rejecting mission request command, component or system ID mismatch"); "Rejecting mission request command, component or system ID mismatch");
@@ -1000,7 +1000,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy\t"); _mavlink.send_statustext_critical("WPM: REJ. CMD: Busy\t");
events::send(events::ID("mavlink_mission_getlist_busy"), events::Log::Error, events::send(events::ID("mavlink_mission_getlist_busy"), events::Log::Error,
"Mission upload busy, already receiving waypoint"); "Mission upload busy, already receiving waypoint");
@@ -1011,7 +1011,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, state %i", _state); PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, state %i", _state);
_mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy\t"); _mavlink.send_statustext_critical("WPM: IGN MISSION_COUNT: Busy\t");
events::send(events::ID("mavlink_mission_ignore_mis_count"), events::Log::Error, events::send(events::ID("mavlink_mission_ignore_mis_count"), events::Log::Error,
"Mission upload busy, ignoring MISSION_COUNT"); "Mission upload busy, ignoring MISSION_COUNT");
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -1088,7 +1088,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer");
_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer\t"); _mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t");
events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error, events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error,
"Ignoring mission item, no transfer in progress"); "Ignoring mission item, no transfer in progress");
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -1099,7 +1099,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
} else { } else {
PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state); PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state);
_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy\t"); _mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t");
events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error, events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error,
"Ignoring mission item, busy"); "Ignoring mission item, busy");
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -1113,7 +1113,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
if (ret != PX4_OK) { if (ret != PX4_OK) {
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
_mavlink->send_statustext_critical("IGN MISSION_ITEM: Invalid item\t"); _mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t");
events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error, events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error,
"Ignoring mission item, invalid item"); "Ignoring mission item, invalid item");
@@ -1208,7 +1208,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
break; break;
default: default:
_mavlink->send_statustext_critical("Received unknown mission type, abort.\t"); _mavlink.send_statustext_critical("Received unknown mission type, abort.\t");
events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error, events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error,
"Received unknown mission type, abort"); "Received unknown mission type, abort");
break; break;
@@ -1220,7 +1220,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
if (write_failed) { if (write_failed) {
_mavlink->send_statustext_critical("Unable to write on micro SD\t"); _mavlink.send_statustext_critical("Unable to write on micro SD\t");
events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error, events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error,
"Mission: unable to write to storage"); "Mission: unable to write to storage");
} }
@@ -1361,7 +1361,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
} }
} else { } else {
_mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy\t"); _mavlink.send_statustext_critical("WPM: IGN CLEAR CMD: Busy\t");
events::send(events::ID("mavlink_mission_ignore_clear"), events::Log::Error, events::send(events::ID("mavlink_mission_ignore_clear"), events::Log::Error,
"Ignoring mission clear command, busy"); "Ignoring mission clear command, busy");
@@ -1839,7 +1839,7 @@ void MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(struct miss
void MavlinkMissionManager::check_active_mission() void MavlinkMissionManager::check_active_mission()
{ {
// do not send anything over high latency communication // do not send anything over high latency communication
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { if (_mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
return; return;
} }
src/modules/mavlink/mavlink_mission.h
+2 -2
View File
@@ -77,7 +77,7 @@ class Mavlink;
class MavlinkMissionManager class MavlinkMissionManager
{ {
public: public:
explicit MavlinkMissionManager(Mavlink *mavlink); explicit MavlinkMissionManager(Mavlink &mavlink);
~MavlinkMissionManager() = default; ~MavlinkMissionManager() = default;
@@ -146,7 +146,7 @@ private:
MavlinkRateLimiter _slow_rate_limiter{1000 * 1000}; ///< Rate limit sending of the current WP sequence to 1 Hz MavlinkRateLimiter _slow_rate_limiter{1000 * 1000}; ///< Rate limit sending of the current WP sequence to 1 Hz
Mavlink *_mavlink; Mavlink &_mavlink;
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
2; ///< Error count limit before stopping to report FS errors 2; ///< Error count limit before stopping to report FS errors
src/modules/mavlink/mavlink_parameters.cpp
+14 -14
View File
@@ -46,7 +46,7 @@
#include "mavlink_main.h" #include "mavlink_main.h"
#include <lib/systemlib/mavlink_log.h> #include <lib/systemlib/mavlink_log.h>
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkParametersManager::MavlinkParametersManager(Mavlink &mavlink) :
_mavlink(mavlink) _mavlink(mavlink)
{ {
} }
@@ -111,7 +111,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Whatever the value is, we're being told to stop sending */ /* Whatever the value is, we're being told to stop sending */
if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) {
if (_mavlink->hash_check_enabled()) { if (_mavlink.hash_check_enabled()) {
_send_all_index = -1; _send_all_index = -1;
} }
@@ -189,7 +189,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
param_value.param_type = MAV_PARAM_TYPE_UINT32; param_value.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&param_value.param_value, &hash, sizeof(hash)); memcpy(&param_value.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value); mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &param_value);
} else { } else {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
@@ -249,7 +249,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
size_t i = map_rc.parameter_rc_channel_index; size_t i = map_rc.parameter_rc_channel_index;
if (i >= sizeof(_rc_param_map.param_index) / sizeof(_rc_param_map.param_index[0])) { if (i >= sizeof(_rc_param_map.param_index) / sizeof(_rc_param_map.param_index[0])) {
mavlink_log_warning(_mavlink->get_mavlink_log_pub(), "parameter_rc_channel_index out of bounds\t"); mavlink_log_warning(_mavlink.get_mavlink_log_pub(), "parameter_rc_channel_index out of bounds\t");
events::send(events::ID("mavlink_param_rc_chan_out_of_bounds"), events::Log::Warning, events::send(events::ID("mavlink_param_rc_chan_out_of_bounds"), events::Log::Warning,
"parameter_rc_channel_index out of bounds"); "parameter_rc_channel_index out of bounds");
break; break;
@@ -319,7 +319,7 @@ MavlinkParametersManager::send()
int max_num_to_send; int max_num_to_send;
if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) { if (_mavlink.get_protocol() == Protocol::SERIAL && !_mavlink.is_usb_uart()) {
max_num_to_send = 3; max_num_to_send = 3;
} else { } else {
@@ -330,7 +330,7 @@ MavlinkParametersManager::send()
int i = 0; int i = 0;
// Send while burst is not exceeded, we still have buffer space and still something to send // Send while burst is not exceeded, we still have buffer space and still something to send
while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() while ((i++ < max_num_to_send) && (_mavlink.get_free_tx_buf() >= get_size()) && !_mavlink.radio_status_critical()
&& send_params()) {} && send_params()) {}
} }
@@ -394,7 +394,7 @@ MavlinkParametersManager::send_untransmitted()
break; break;
} }
} }
} while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() } while ((_mavlink.get_free_tx_buf() >= get_size()) && !_mavlink.radio_status_critical()
&& (_param_update_index < (int) param_count())); && (_param_update_index < (int) param_count()));
// Flag work as done once all params have been sent // Flag work as done once all params have been sent
@@ -426,7 +426,7 @@ MavlinkParametersManager::send_one()
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32; msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash)); memcpy(&msg.param_value, &hash, sizeof(hash));
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &msg);
/* after this we should start sending all params */ /* after this we should start sending all params */
_send_all_index = 0; _send_all_index = 0;
@@ -468,7 +468,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
} }
/* no free TX buf to send this param */ /* no free TX buf to send this param */
if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) { if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
return 1; return 1;
} }
@@ -535,13 +535,13 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
/* default component ID */ /* default component ID */
if (component_id < 0) { if (component_id < 0) {
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &msg);
} else { } else {
// Re-pack the message with a different component ID // Re-pack the message with a different component ID
mavlink_message_t mavlink_packet; mavlink_message_t mavlink_packet;
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg); mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink.get_channel(), &mavlink_packet, &msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); _mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
} }
return 0; return 0;
@@ -595,9 +595,9 @@ bool MavlinkParametersManager::send_uavcan()
// Re-pack the message with the UAVCAN node ID // Re-pack the message with the UAVCAN node ID
mavlink_message_t mavlink_packet{}; mavlink_message_t mavlink_packet{};
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink.get_channel(), &mavlink_packet,
&msg); &msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); _mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
return true; return true;
} }
src/modules/mavlink/mavlink_parameters.h
+2 -2
View File
@@ -64,7 +64,7 @@ class Mavlink;
class MavlinkParametersManager class MavlinkParametersManager
{ {
public: public:
explicit MavlinkParametersManager(Mavlink *mavlink); explicit MavlinkParametersManager(Mavlink &mavlink);
~MavlinkParametersManager() = default; ~MavlinkParametersManager() = default;
/** /**
@@ -159,7 +159,7 @@ protected:
hrt_abstime _param_update_time{0}; hrt_abstime _param_update_time{0};
int _param_update_index{0}; int _param_update_index{0};
Mavlink *_mavlink; Mavlink &_mavlink;
bool _first_send{false}; bool _first_send{false};
}; };
src/modules/mavlink/mavlink_receiver.cpp
+73 -73
View File
@@ -106,7 +106,7 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty {
.quality = 0 .quality = 0
}; };
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : MavlinkReceiver::MavlinkReceiver(Mavlink &parent) :
ModuleParams(nullptr), ModuleParams(nullptr),
_mavlink(parent), _mavlink(parent),
_mavlink_ftp(parent), _mavlink_ftp(parent),
@@ -344,7 +344,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
* Accept HIL GPS messages if use_hil_gps flag is true. * Accept HIL GPS messages if use_hil_gps flag is true.
* This allows to provide fake gps measurements to the system. * This allows to provide fake gps measurements to the system.
*/ */
if (_mavlink->get_hil_enabled()) { if (_mavlink.get_hil_enabled()) {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_SENSOR: case MAVLINK_MSG_ID_HIL_SENSOR:
handle_message_hil_sensor(msg); handle_message_hil_sensor(msg);
@@ -364,7 +364,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
} }
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { if (_mavlink.get_hil_enabled() || (_mavlink.get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_GPS: case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg); handle_message_hil_gps(msg);
@@ -380,18 +380,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
_mission_manager.handle_message(msg); _mission_manager.handle_message(msg);
/* handle packet with parameter component */ /* handle packet with parameter component */
if (_mavlink->boot_complete()) { if (_mavlink.boot_complete()) {
// make sure mavlink app has booted before we start processing parameter sync // make sure mavlink app has booted before we start processing parameter sync
_parameters_manager.handle_message(msg); _parameters_manager.handle_message(msg);
} else { } else {
if (hrt_elapsed_time(&_mavlink->get_first_start_time()) > 20_s) { if (hrt_elapsed_time(&_mavlink.get_first_start_time()) > 20_s) {
PX4_ERR("system boot did not complete in 20 seconds"); PX4_ERR("system boot did not complete in 20 seconds");
_mavlink->set_boot_complete(); _mavlink.set_boot_complete();
} }
} }
if (_mavlink->ftp_enabled()) { if (_mavlink.ftp_enabled()) {
/* handle packet with ftp component */ /* handle packet with ftp component */
_mavlink_ftp.handle_message(msg); _mavlink_ftp.handle_message(msg);
} }
@@ -403,7 +403,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
_mavlink_timesync.handle_message(msg); _mavlink_timesync.handle_message(msg);
/* handle packet with parent object */ /* handle packet with parent object */
_mavlink->handle_message(msg); _mavlink.handle_message(msg);
} }
bool bool
@@ -527,8 +527,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
if (!target_ok) { if (!target_ok) {
// Reject alien commands only if there is no forwarding or we've never seen target component before // Reject alien commands only if there is no forwarding or we've never seen target component before
if (!_mavlink->get_forwarding_on() if (!_mavlink.get_forwarding_on()
|| !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { || !_mavlink.component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
} }
@@ -568,7 +568,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) { } else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
if (_mavlink->failure_injection_enabled()) { if (_mavlink.failure_injection_enabled()) {
_cmd_pub.publish(vehicle_command); _cmd_pub.publish(vehicle_command);
send_ack = false; send_ack = false;
@@ -694,19 +694,19 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
// check that we have enough bandwidth available: this is given by the configured logger topics // check that we have enough bandwidth available: this is given by the configured logger topics
// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming // and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming
// on a radio link // on a radio link
if (_mavlink->get_data_rate() < 5000) { if (_mavlink.get_data_rate() < 5000) {
send_ack = true; send_ack = true;
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming\t"); _mavlink.send_statustext_critical("Not enough bandwidth to enable log streaming\t");
events::send<uint32_t>(events::ID("mavlink_log_not_enough_bw"), events::Log::Error, events::send<uint32_t>(events::ID("mavlink_log_not_enough_bw"), events::Log::Error,
"Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink->get_data_rate()); "Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink.get_data_rate());
} else { } else {
// we already instanciate the streaming object, because at this point we know on which // we already instanciate the streaming object, because at this point we know on which
// mavlink channel streaming was requested. But in fact it's possible that the logger is // mavlink channel streaming was requested. But in fact it's possible that the logger is
// not even running. The main mavlink thread takes care of this by waiting for an ack // not even running. The main mavlink thread takes care of this by waiting for an ack
// from the logger. // from the logger.
_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); _mavlink.try_start_ulog_streaming(msg->sysid, msg->compid);
} }
} }
@@ -726,7 +726,7 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo
bool stream_found = false; bool stream_found = false;
bool message_sent = false; bool message_sent = false;
for (const auto &stream : _mavlink->get_streams()) { for (const auto &stream : _mavlink.get_streams()) {
if (stream->get_id() == message_id) { if (stream->get_id() == message_id) {
stream_found = true; stream_found = true;
message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); message_sent = stream->request_message(param2, param3, param4, param5, param6, param7);
@@ -739,10 +739,10 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo
const char *stream_name = get_stream_name(message_id); const char *stream_name = get_stream_name(message_id);
if (stream_name != nullptr) { if (stream_name != nullptr) {
_mavlink->configure_stream_threadsafe(stream_name, 0.0f); _mavlink.configure_stream_threadsafe(stream_name, 0.0f);
// Now we try again to send it. // Now we try again to send it.
for (const auto &stream : _mavlink->get_streams()) { for (const auto &stream : _mavlink.get_streams()) {
if (stream->get_id() == message_id) { if (stream->get_id() == message_id) {
message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); message_sent = stream->request_message(param2, param3, param4, param5, param6, param7);
break; break;
@@ -762,7 +762,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
mavlink_command_ack_t ack; mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(msg, &ack); mavlink_msg_command_ack_decode(msg, &ack);
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink.get_channel());
vehicle_command_ack_s command_ack{}; vehicle_command_ack_s command_ack{};
@@ -793,7 +793,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
device::Device::DeviceId device_id; device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.bus = _mavlink.get_instance_id();
device_id.devid_s.address = msg->sysid; device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK; device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK;
@@ -839,7 +839,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
device::Device::DeviceId device_id; device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.bus = _mavlink.get_instance_id();
device_id.devid_s.address = msg->sysid; device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
@@ -916,7 +916,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
device::Device::DeviceId device_id; device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.bus = _mavlink.get_instance_id();
device_id.devid_s.address = msg->sysid; device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
@@ -988,7 +988,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned); mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned);
/* Only accept messages which are intended for this system */ /* Only accept messages which are intended for this system */
if (_mavlink->get_forward_externalsp() && if (_mavlink.get_forward_externalsp() &&
(mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) && (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) &&
(mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) { (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) {
@@ -1108,7 +1108,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int); mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int);
/* Only accept messages which are intended for this system */ /* Only accept messages which are intended for this system */
if (_mavlink->get_forward_externalsp() && if (_mavlink.get_forward_externalsp() &&
(mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) && (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) &&
(mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) { (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) {
@@ -1224,13 +1224,13 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
mavlink_set_gps_global_origin_t gps_global_origin; mavlink_set_gps_global_origin_t gps_global_origin;
mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin); mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin);
if (gps_global_origin.target_system == _mavlink->get_system_id()) { if (gps_global_origin.target_system == _mavlink.get_system_id()) {
vehicle_command_s vcmd{}; vehicle_command_s vcmd{};
vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7; vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7;
vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7; vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7;
vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f; vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f;
vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN; vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN;
vcmd.target_system = _mavlink->get_system_id(); vcmd.target_system = _mavlink.get_system_id();
vcmd.target_component = MAV_COMP_ID_ALL; vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid; vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
@@ -1514,7 +1514,7 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type
{ {
// Fill correct field by checking frametype // Fill correct field by checking frametype
// TODO: add as needed // TODO: add as needed
switch (_mavlink->get_system_type()) { switch (_mavlink.get_system_type()) {
case MAV_TYPE_GENERIC: case MAV_TYPE_GENERIC:
break; break;
@@ -1570,7 +1570,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
mavlink_msg_set_attitude_target_decode(msg, &attitude_target); mavlink_msg_set_attitude_target_decode(msg, &attitude_target);
/* Only accept messages which are intended for this system */ /* Only accept messages which are intended for this system */
if (_mavlink->get_forward_externalsp() && if (_mavlink.get_forward_externalsp() &&
(mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) && (mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) &&
(mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) { (mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) {
@@ -1659,7 +1659,7 @@ void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{ {
/* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ /* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
mavlink_radio_status_t rstatus; mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus); mavlink_msg_radio_status_decode(msg, &rstatus);
@@ -1674,7 +1674,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
status.rxerrors = rstatus.rxerrors; status.rxerrors = rstatus.rxerrors;
status.fix = rstatus.fixed; status.fix = rstatus.fixed;
_mavlink->update_radio_status(status); _mavlink.update_radio_status(status);
_radio_status_pub.publish(status); _radio_status_pub.publish(status);
} }
@@ -1691,7 +1691,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
ping.target_system = msg->sysid; ping.target_system = msg->sysid;
ping.target_component = msg->compid; ping.target_component = msg->compid;
mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping); mavlink_msg_ping_send_struct(_mavlink.get_channel(), &ping);
} else if ((ping.target_system == mavlink_system.sysid) && } else if ((ping.target_system == mavlink_system.sysid) &&
(ping.target_component == (ping.target_component ==
@@ -1703,7 +1703,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
float rtt_ms = (now - ping.time_usec) / 1000.0f; float rtt_ms = (now - ping.time_usec) / 1000.0f;
// Update ping statistics // Update ping statistics
struct Mavlink::ping_statistics_s &pstats = _mavlink->get_ping_statistics(); struct Mavlink::ping_statistics_s &pstats = _mavlink.get_ping_statistics();
pstats.last_ping_time = now; pstats.last_ping_time = now;
@@ -1728,7 +1728,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms; pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms;
/* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ /* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
ping_s uorb_ping_msg{}; ping_s uorb_ping_msg{};
@@ -1814,7 +1814,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
return; return;
} }
MavlinkShell *shell = _mavlink->get_shell(); MavlinkShell *shell = _mavlink.get_shell();
if (shell) { if (shell) {
// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
@@ -1825,7 +1825,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
// if no response requested, assume the shell is no longer used // if no response requested, assume the shell is no longer used
if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) { if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
_mavlink->close_shell(); _mavlink.close_shell();
} }
} }
} }
@@ -1836,7 +1836,7 @@ MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg)
mavlink_logging_ack_t logging_ack; mavlink_logging_ack_t logging_ack;
mavlink_msg_logging_ack_decode(msg, &logging_ack); mavlink_msg_logging_ack_decode(msg, &logging_ack);
MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming(); MavlinkULog *ulog_streaming = _mavlink.get_ulog_streaming();
if (ulog_streaming) { if (ulog_streaming) {
ulog_streaming->handle_ack(logging_ack); ulog_streaming->handle_ack(logging_ack);
@@ -2013,7 +2013,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
mavlink_msg_rc_channels_override_decode(msg, &man); mavlink_msg_rc_channels_override_decode(msg, &man);
// Check target // Check target
if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { if (man.target_system != 0 && man.target_system != _mavlink.get_system_id()) {
return; return;
} }
@@ -2100,7 +2100,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_msg_manual_control_decode(msg, &mavlink_manual_control); mavlink_msg_manual_control_decode(msg, &mavlink_manual_control);
// Check target // Check target
if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink->get_system_id()) { if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink.get_system_id()) {
return; return;
} }
@@ -2112,7 +2112,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
// Pass along the button states // Pass along the button states
manual_control_setpoint.buttons = mavlink_manual_control.buttons; manual_control_setpoint.buttons = mavlink_manual_control.buttons;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id();
manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
manual_control_setpoint.valid = true; manual_control_setpoint.valid = true;
_manual_control_input_pub.publish(manual_control_setpoint); _manual_control_input_pub.publish(manual_control_setpoint);
@@ -2122,7 +2122,7 @@ void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{ {
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
const hrt_abstime now = hrt_absolute_time(); const hrt_abstime now = hrt_absolute_time();
@@ -2166,13 +2166,13 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
case MAV_TYPE_PARACHUTE: case MAV_TYPE_PARACHUTE:
_heartbeat_type_parachute = now; _heartbeat_type_parachute = now;
_mavlink->telemetry_status().parachute_system_healthy = _mavlink.telemetry_status().parachute_system_healthy =
(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
break; break;
case MAV_TYPE_ODID: case MAV_TYPE_ODID:
_heartbeat_type_open_drone_id = now; _heartbeat_type_open_drone_id = now;
_mavlink->telemetry_status().open_drone_id_system_healthy = _mavlink.telemetry_status().open_drone_id_system_healthy =
(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
break; break;
@@ -2197,7 +2197,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
case MAV_COMP_ID_OBSTACLE_AVOIDANCE: case MAV_COMP_ID_OBSTACLE_AVOIDANCE:
_heartbeat_component_obstacle_avoidance = now; _heartbeat_component_obstacle_avoidance = now;
_mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE);
break; break;
case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY:
@@ -2234,7 +2234,7 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
} }
if (data_rate > 0) { if (data_rate > 0) {
_mavlink->set_data_rate(data_rate); _mavlink.set_data_rate(data_rate);
} }
// configure_stream wants a rate (msgs/second), so convert here. // configure_stream wants a rate (msgs/second), so convert here.
@@ -2256,7 +2256,7 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
const char *stream_name = get_stream_name(msgId); const char *stream_name = get_stream_name(msgId);
if (stream_name != nullptr) { if (stream_name != nullptr) {
_mavlink->configure_stream_threadsafe(stream_name, rate); _mavlink.configure_stream_threadsafe(stream_name, rate);
found_id = true; found_id = true;
} }
} }
@@ -2269,7 +2269,7 @@ MavlinkReceiver::get_message_interval(int msgId)
{ {
unsigned interval = 0; unsigned interval = 0;
for (const auto &stream : _mavlink->get_streams()) { for (const auto &stream : _mavlink.get_streams()) {
if (stream->get_id() == msgId) { if (stream->get_id() == msgId) {
interval = stream->get_interval(); interval = stream->get_interval();
break; break;
@@ -2277,7 +2277,7 @@ MavlinkReceiver::get_message_interval(int msgId)
} }
// send back this value... // send back this value...
mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); mavlink_msg_message_interval_send(_mavlink.get_channel(), msgId, interval);
} }
void void
@@ -2394,7 +2394,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
device::Device::DeviceId device_id; device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.bus = _mavlink.get_instance_id();
device_id.devid_s.address = msg->sysid; device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
@@ -2908,7 +2908,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
} }
if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) { if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) {
telemetry_status_s &tstatus = _mavlink->telemetry_status(); telemetry_status_s &tstatus = _mavlink.telemetry_status();
tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker); tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker);
tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs); tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs);
@@ -2928,14 +2928,14 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge);
tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge); tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge);
_mavlink->telemetry_status_updated(); _mavlink.telemetry_status_updated();
_last_heartbeat_check = t; _last_heartbeat_check = t;
} }
} }
void MavlinkReceiver::handle_message_request_event(mavlink_message_t *msg) void MavlinkReceiver::handle_message_request_event(mavlink_message_t *msg)
{ {
_mavlink->get_events_protocol().handle_request_event(*msg); _mavlink.get_events_protocol().handle_request_event(*msg);
} }
void void
@@ -3058,7 +3058,7 @@ MavlinkReceiver::run()
/* set thread name */ /* set thread name */
{ {
char thread_name[17]; char thread_name[17];
snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink->get_instance_id()); snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink.get_instance_id());
px4_prctl(PR_SET_NAME, thread_name, px4_getpid()); px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
} }
@@ -3079,8 +3079,8 @@ MavlinkReceiver::run()
struct pollfd fds[1] = {}; struct pollfd fds[1] = {};
if (_mavlink->get_protocol() == Protocol::SERIAL) { if (_mavlink.get_protocol() == Protocol::SERIAL) {
fds[0].fd = _mavlink->get_uart_fd(); fds[0].fd = _mavlink.get_uart_fd();
fds[0].events = POLLIN; fds[0].events = POLLIN;
} }
@@ -3088,8 +3088,8 @@ MavlinkReceiver::run()
struct sockaddr_in srcaddr = {}; struct sockaddr_in srcaddr = {};
socklen_t addrlen = sizeof(srcaddr); socklen_t addrlen = sizeof(srcaddr);
if (_mavlink->get_protocol() == Protocol::UDP) { if (_mavlink.get_protocol() == Protocol::UDP) {
fds[0].fd = _mavlink->get_socket_fd(); fds[0].fd = _mavlink.get_socket_fd();
fds[0].events = POLLIN; fds[0].events = POLLIN;
} }
@@ -3098,7 +3098,7 @@ MavlinkReceiver::run()
ssize_t nread = 0; ssize_t nread = 0;
hrt_abstime last_send_update = 0; hrt_abstime last_send_update = 0;
while (!_mavlink->should_exit()) { while (!_mavlink.should_exit()) {
// check for parameter updates // check for parameter updates
if (_parameter_update_sub.updated()) { if (_parameter_update_sub.updated()) {
@@ -3113,7 +3113,7 @@ MavlinkReceiver::run()
int ret = poll(&fds[0], 1, timeout); int ret = poll(&fds[0], 1, timeout);
if (ret > 0) { if (ret > 0) {
if (_mavlink->get_protocol() == Protocol::SERIAL) { if (_mavlink.get_protocol() == Protocol::SERIAL) {
/* non-blocking read. read may return negative values */ /* non-blocking read. read may return negative values */
nread = ::read(fds[0].fd, buf, sizeof(buf)); nread = ::read(fds[0].fd, buf, sizeof(buf));
@@ -3124,21 +3124,21 @@ MavlinkReceiver::run()
#if defined(MAVLINK_UDP) #if defined(MAVLINK_UDP)
else if (_mavlink->get_protocol() == Protocol::UDP) { else if (_mavlink.get_protocol() == Protocol::UDP) {
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); nread = recvfrom(_mavlink.get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
} }
struct sockaddr_in &srcaddr_last = _mavlink->get_client_source_address(); struct sockaddr_in &srcaddr_last = _mavlink.get_client_source_address();
int localhost = (127 << 24) + 1; int localhost = (127 << 24) + 1;
if (!_mavlink->get_client_source_initialized()) { if (!_mavlink.get_client_source_initialized()) {
// set the address either if localhost or if 3 seconds have passed // set the address either if localhost or if 3 seconds have passed
// this ensures that a GCS running on localhost can get a hold of // this ensures that a GCS running on localhost can get a hold of
// the system within the first N seconds // the system within the first N seconds
hrt_abstime stime = _mavlink->get_start_time(); hrt_abstime stime = _mavlink.get_start_time();
if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s))
|| (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) {
@@ -3146,7 +3146,7 @@ MavlinkReceiver::run()
srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr;
srcaddr_last.sin_port = srcaddr.sin_port; srcaddr_last.sin_port = srcaddr.sin_port;
_mavlink->set_client_source_initialized(); _mavlink.set_client_source_initialized();
PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr));
} }
@@ -3154,21 +3154,21 @@ MavlinkReceiver::run()
} }
// only start accepting messages on UDP once we're sure who we talk to // only start accepting messages on UDP once we're sure who we talk to
if (_mavlink->get_protocol() != Protocol::UDP || _mavlink->get_client_source_initialized()) { if (_mavlink.get_protocol() != Protocol::UDP || _mavlink.get_client_source_initialized()) {
#endif // MAVLINK_UDP #endif // MAVLINK_UDP
/* if read failed, this loop won't execute */ /* if read failed, this loop won't execute */
for (ssize_t i = 0; i < nread; i++) { for (ssize_t i = 0; i < nread; i++) {
if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) { if (mavlink_parse_char(_mavlink.get_channel(), buf[i], &msg, &_status)) {
/* check if we received version 2 and request a switch. */ /* check if we received version 2 and request a switch. */
if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) { if (!(_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) {
/* this will only switch to proto version 2 if allowed in settings */ /* this will only switch to proto version 2 if allowed in settings */
_mavlink->set_proto_version(2); _mavlink.set_proto_version(2);
} }
handle_message(&msg); handle_message(&msg);
_mavlink->set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag _mavlink.set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag
update_rx_stats(msg); update_rx_stats(msg);
if (_message_statistics_enabled) { if (_message_statistics_enabled) {
@@ -3179,9 +3179,9 @@ MavlinkReceiver::run()
/* count received bytes (nread will be -1 on read error) */ /* count received bytes (nread will be -1 on read error) */
if (nread > 0) { if (nread > 0) {
_mavlink->count_rxbytes(nread); _mavlink.count_rxbytes(nread);
telemetry_status_s &tstatus = _mavlink->telemetry_status(); telemetry_status_s &tstatus = _mavlink.telemetry_status();
tstatus.rx_message_count = _total_received_counter; tstatus.rx_message_count = _total_received_counter;
tstatus.rx_message_lost_count = _total_lost_counter; tstatus.rx_message_lost_count = _total_lost_counter;
tstatus.rx_message_lost_rate = static_cast<float>(_total_lost_counter) / static_cast<float>(_total_received_counter); tstatus.rx_message_lost_rate = static_cast<float>(_total_lost_counter) / static_cast<float>(_total_received_counter);
@@ -3219,11 +3219,11 @@ MavlinkReceiver::run()
_mission_manager.check_active_mission(); _mission_manager.check_active_mission();
_mission_manager.send(); _mission_manager.send();
if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) {
_parameters_manager.send(); _parameters_manager.send();
} }
if (_mavlink->ftp_enabled()) { if (_mavlink.ftp_enabled()) {
_mavlink_ftp.send(); _mavlink_ftp.send();
} }
src/modules/mavlink/mavlink_receiver.h
+2 -2
View File
@@ -126,7 +126,7 @@ class Mavlink;
class MavlinkReceiver : public ModuleParams class MavlinkReceiver : public ModuleParams
{ {
public: public:
MavlinkReceiver(Mavlink *parent); MavlinkReceiver(Mavlink &parent);
~MavlinkReceiver() override; ~MavlinkReceiver() override;
void start(); void start();
@@ -246,7 +246,7 @@ private:
*/ */
void updateParams() override; void updateParams() override;
Mavlink *_mavlink; Mavlink &_mavlink;
MavlinkFTP _mavlink_ftp; MavlinkFTP _mavlink_ftp;
MavlinkLogHandler _mavlink_log_handler; MavlinkLogHandler _mavlink_log_handler;
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+1 -1
View File
@@ -77,7 +77,7 @@ MavlinkFtpTest::MavlinkFtpTest() :
void MavlinkFtpTest::_init() void MavlinkFtpTest::_init()
{ {
_expected_seq_number = 0; _expected_seq_number = 0;
_ftp_server = new MavlinkFTP(nullptr); _ftp_server = new MavlinkFTP(_mavlink);
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
_create_test_files(); _create_test_files();
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+2 -4
View File
@@ -37,12 +37,9 @@
#pragma once #pragma once
#include <unit_test.h> #include <unit_test.h>
#ifndef MAVLINK_FTP_UNIT_TEST
#include "../mavlink_bridge_header.h" #include "../mavlink_bridge_header.h"
#else
#include <mavlink.h>
#endif
#include "../mavlink_ftp.h" #include "../mavlink_ftp.h"
#include "../mavlink_main.h"
class MavlinkFtpTest : public UnitTest class MavlinkFtpTest : public UnitTest
{ {
@@ -130,6 +127,7 @@ private:
bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info); bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info);
MavlinkFTP *_ftp_server; MavlinkFTP *_ftp_server;
Mavlink _mavlink;
uint16_t _expected_seq_number; uint16_t _expected_seq_number;
mavlink_file_transfer_protocol_t _reply_msg; mavlink_file_transfer_protocol_t _reply_msg;
src/modules/mavlink/mavlink_timesync.cpp
+2 -2
View File
@@ -43,7 +43,7 @@
#include <stdlib.h> #include <stdlib.h>
MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : MavlinkTimesync::MavlinkTimesync(Mavlink &mavlink) :
_mavlink(mavlink) _mavlink(mavlink)
{ {
} }
@@ -66,7 +66,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg)
rsync.tc1 = now * 1000ULL; rsync.tc1 = now * 1000ULL;
rsync.ts1 = tsync.ts1; rsync.ts1 = tsync.ts1;
mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); mavlink_msg_timesync_send_struct(_mavlink.get_channel(), &rsync);
return; return;
src/modules/mavlink/mavlink_timesync.h
+2 -2
View File
@@ -49,7 +49,7 @@ class Mavlink;
class MavlinkTimesync class MavlinkTimesync
{ {
public: public:
explicit MavlinkTimesync(Mavlink *mavlink); explicit MavlinkTimesync(Mavlink &mavlink);
~MavlinkTimesync() = default; ~MavlinkTimesync() = default;
void handle_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg);
@@ -61,6 +61,6 @@ public:
uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); } uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); }
private: private:
Mavlink *const _mavlink; Mavlink &_mavlink;
Timesync _timesync{}; Timesync _timesync{};
}; };