mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 20:37:35 +08:00
mavlink: use reference instead of pointer to access the MAVLink instance from protocol classes
This commit is contained in:
@@ -46,7 +46,7 @@
|
||||
#include "mavlink_main.h"
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
|
||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
|
||||
MavlinkParametersManager::MavlinkParametersManager(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 */
|
||||
if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) {
|
||||
|
||||
if (_mavlink->hash_check_enabled()) {
|
||||
if (_mavlink.hash_check_enabled()) {
|
||||
_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);
|
||||
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);
|
||||
mavlink_msg_param_value_send_struct(_mavlink.get_channel(), ¶m_value);
|
||||
|
||||
} else {
|
||||
/* 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;
|
||||
|
||||
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,
|
||||
"parameter_rc_channel_index out of bounds");
|
||||
break;
|
||||
@@ -319,7 +319,7 @@ MavlinkParametersManager::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;
|
||||
|
||||
} else {
|
||||
@@ -330,7 +330,7 @@ MavlinkParametersManager::send()
|
||||
int i = 0;
|
||||
|
||||
// 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()) {}
|
||||
}
|
||||
|
||||
@@ -394,7 +394,7 @@ MavlinkParametersManager::send_untransmitted()
|
||||
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()));
|
||||
|
||||
// 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);
|
||||
msg.param_type = MAV_PARAM_TYPE_UINT32;
|
||||
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 */
|
||||
_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 */
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -535,13 +535,13 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
|
||||
|
||||
/* default component ID */
|
||||
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 {
|
||||
// Re-pack the message with a different component ID
|
||||
mavlink_message_t mavlink_packet;
|
||||
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_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink.get_channel(), &mavlink_packet, &msg);
|
||||
_mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -595,9 +595,9 @@ bool MavlinkParametersManager::send_uavcan()
|
||||
|
||||
// Re-pack the message with the UAVCAN node ID
|
||||
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);
|
||||
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
|
||||
_mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user