mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-07 09:50:35 +08:00
mavlink: use reference instead of pointer to access the MAVLink instance from protocol classes
This commit is contained in:
Matthias Grob
@@ -106,7 +106,7 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty {
|
||||
.quality = 0
|
||||
};
|
||||
|
||||
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
MavlinkReceiver::MavlinkReceiver(Mavlink &parent) :
|
||||
ModuleParams(nullptr),
|
||||
_mavlink(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.
|
||||
* This allows to provide fake gps measurements to the system.
|
||||
*/
|
||||
if (_mavlink->get_hil_enabled()) {
|
||||
if (_mavlink.get_hil_enabled()) {
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
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) {
|
||||
case MAVLINK_MSG_ID_HIL_GPS:
|
||||
handle_message_hil_gps(msg);
|
||||
@@ -380,18 +380,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
_mission_manager.handle_message(msg);
|
||||
|
||||
/* 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
|
||||
_parameters_manager.handle_message(msg);
|
||||
|
||||
} 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");
|
||||
_mavlink->set_boot_complete();
|
||||
_mavlink.set_boot_complete();
|
||||
}
|
||||
}
|
||||
|
||||
if (_mavlink->ftp_enabled()) {
|
||||
if (_mavlink.ftp_enabled()) {
|
||||
/* handle packet with ftp component */
|
||||
_mavlink_ftp.handle_message(msg);
|
||||
}
|
||||
@@ -403,7 +403,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
_mavlink_timesync.handle_message(msg);
|
||||
|
||||
/* handle packet with parent object */
|
||||
_mavlink->handle_message(msg);
|
||||
_mavlink.handle_message(msg);
|
||||
}
|
||||
|
||||
bool
|
||||
@@ -527,8 +527,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
|
||||
|
||||
if (!target_ok) {
|
||||
// Reject alien commands only if there is no forwarding or we've never seen target component before
|
||||
if (!_mavlink->get_forwarding_on()
|
||||
|| !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
|
||||
if (!_mavlink.get_forwarding_on()
|
||||
|| !_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);
|
||||
}
|
||||
|
||||
@@ -568,7 +568,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
|
||||
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
|
||||
|
||||
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
|
||||
if (_mavlink->failure_injection_enabled()) {
|
||||
if (_mavlink.failure_injection_enabled()) {
|
||||
_cmd_pub.publish(vehicle_command);
|
||||
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
|
||||
// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming
|
||||
// on a radio link
|
||||
if (_mavlink->get_data_rate() < 5000) {
|
||||
if (_mavlink.get_data_rate() < 5000) {
|
||||
send_ack = true;
|
||||
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,
|
||||
"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 {
|
||||
// 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
|
||||
// not even running. The main mavlink thread takes care of this by waiting for an ack
|
||||
// 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 message_sent = false;
|
||||
|
||||
for (const auto &stream : _mavlink->get_streams()) {
|
||||
for (const auto &stream : _mavlink.get_streams()) {
|
||||
if (stream->get_id() == message_id) {
|
||||
stream_found = true;
|
||||
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);
|
||||
|
||||
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.
|
||||
for (const auto &stream : _mavlink->get_streams()) {
|
||||
for (const auto &stream : _mavlink.get_streams()) {
|
||||
if (stream->get_id() == message_id) {
|
||||
message_sent = stream->request_message(param2, param3, param4, param5, param6, param7);
|
||||
break;
|
||||
@@ -762,7 +762,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
|
||||
mavlink_command_ack_t 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{};
|
||||
|
||||
@@ -793,7 +793,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
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.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_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.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
@@ -916,7 +916,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
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.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);
|
||||
|
||||
/* 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.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);
|
||||
|
||||
/* 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.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_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{};
|
||||
vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7;
|
||||
vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7;
|
||||
vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f;
|
||||
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.source_system = msg->sysid;
|
||||
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
|
||||
// TODO: add as needed
|
||||
switch (_mavlink->get_system_type()) {
|
||||
switch (_mavlink.get_system_type()) {
|
||||
case MAV_TYPE_GENERIC:
|
||||
break;
|
||||
|
||||
@@ -1570,7 +1570,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
mavlink_msg_set_attitude_target_decode(msg, &attitude_target);
|
||||
|
||||
/* 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.compid == attitude_target.target_component || attitude_target.target_component == 0)) {
|
||||
|
||||
@@ -1659,7 +1659,7 @@ void
|
||||
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
||||
{
|
||||
/* 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_msg_radio_status_decode(msg, &rstatus);
|
||||
|
||||
@@ -1674,7 +1674,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
||||
status.rxerrors = rstatus.rxerrors;
|
||||
status.fix = rstatus.fixed;
|
||||
|
||||
_mavlink->update_radio_status(status);
|
||||
_mavlink.update_radio_status(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_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) &&
|
||||
(ping.target_component ==
|
||||
@@ -1703,7 +1703,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
|
||||
float rtt_ms = (now - ping.time_usec) / 1000.0f;
|
||||
|
||||
// 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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
/* 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{};
|
||||
|
||||
@@ -1814,7 +1814,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
|
||||
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
|
||||
@@ -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 ((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_msg_logging_ack_decode(msg, &logging_ack);
|
||||
|
||||
MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming();
|
||||
MavlinkULog *ulog_streaming = _mavlink.get_ulog_streaming();
|
||||
|
||||
if (ulog_streaming) {
|
||||
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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
@@ -2100,7 +2100,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
||||
mavlink_msg_manual_control_decode(msg, &mavlink_manual_control);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
@@ -2112,7 +2112,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
||||
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
|
||||
// Pass along the button states
|
||||
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.valid = true;
|
||||
_manual_control_input_pub.publish(manual_control_setpoint);
|
||||
@@ -2122,7 +2122,7 @@ void
|
||||
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
{
|
||||
/* 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();
|
||||
|
||||
@@ -2166,13 +2166,13 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
|
||||
case MAV_TYPE_PARACHUTE:
|
||||
_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);
|
||||
break;
|
||||
|
||||
case MAV_TYPE_ODID:
|
||||
_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);
|
||||
break;
|
||||
|
||||
@@ -2197,7 +2197,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
|
||||
case MAV_COMP_ID_OBSTACLE_AVOIDANCE:
|
||||
_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;
|
||||
|
||||
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) {
|
||||
_mavlink->set_data_rate(data_rate);
|
||||
_mavlink.set_data_rate(data_rate);
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
if (stream_name != nullptr) {
|
||||
_mavlink->configure_stream_threadsafe(stream_name, rate);
|
||||
_mavlink.configure_stream_threadsafe(stream_name, rate);
|
||||
found_id = true;
|
||||
}
|
||||
}
|
||||
@@ -2269,7 +2269,7 @@ MavlinkReceiver::get_message_interval(int msgId)
|
||||
{
|
||||
unsigned interval = 0;
|
||||
|
||||
for (const auto &stream : _mavlink->get_streams()) {
|
||||
for (const auto &stream : _mavlink.get_streams()) {
|
||||
if (stream->get_id() == msgId) {
|
||||
interval = stream->get_interval();
|
||||
break;
|
||||
@@ -2277,7 +2277,7 @@ MavlinkReceiver::get_message_interval(int msgId)
|
||||
}
|
||||
|
||||
// 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
|
||||
@@ -2394,7 +2394,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
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.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) {
|
||||
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_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_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge);
|
||||
|
||||
_mavlink->telemetry_status_updated();
|
||||
_mavlink.telemetry_status_updated();
|
||||
_last_heartbeat_check = t;
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
@@ -3058,7 +3058,7 @@ MavlinkReceiver::run()
|
||||
/* set thread name */
|
||||
{
|
||||
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());
|
||||
}
|
||||
|
||||
@@ -3079,8 +3079,8 @@ MavlinkReceiver::run()
|
||||
|
||||
struct pollfd fds[1] = {};
|
||||
|
||||
if (_mavlink->get_protocol() == Protocol::SERIAL) {
|
||||
fds[0].fd = _mavlink->get_uart_fd();
|
||||
if (_mavlink.get_protocol() == Protocol::SERIAL) {
|
||||
fds[0].fd = _mavlink.get_uart_fd();
|
||||
fds[0].events = POLLIN;
|
||||
}
|
||||
|
||||
@@ -3088,8 +3088,8 @@ MavlinkReceiver::run()
|
||||
struct sockaddr_in srcaddr = {};
|
||||
socklen_t addrlen = sizeof(srcaddr);
|
||||
|
||||
if (_mavlink->get_protocol() == Protocol::UDP) {
|
||||
fds[0].fd = _mavlink->get_socket_fd();
|
||||
if (_mavlink.get_protocol() == Protocol::UDP) {
|
||||
fds[0].fd = _mavlink.get_socket_fd();
|
||||
fds[0].events = POLLIN;
|
||||
}
|
||||
|
||||
@@ -3098,7 +3098,7 @@ MavlinkReceiver::run()
|
||||
ssize_t nread = 0;
|
||||
hrt_abstime last_send_update = 0;
|
||||
|
||||
while (!_mavlink->should_exit()) {
|
||||
while (!_mavlink.should_exit()) {
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
@@ -3113,7 +3113,7 @@ MavlinkReceiver::run()
|
||||
int ret = poll(&fds[0], 1, timeout);
|
||||
|
||||
if (ret > 0) {
|
||||
if (_mavlink->get_protocol() == Protocol::SERIAL) {
|
||||
if (_mavlink.get_protocol() == Protocol::SERIAL) {
|
||||
/* non-blocking read. read may return negative values */
|
||||
nread = ::read(fds[0].fd, buf, sizeof(buf));
|
||||
|
||||
@@ -3124,21 +3124,21 @@ MavlinkReceiver::run()
|
||||
|
||||
#if defined(MAVLINK_UDP)
|
||||
|
||||
else if (_mavlink->get_protocol() == Protocol::UDP) {
|
||||
else if (_mavlink.get_protocol() == Protocol::UDP) {
|
||||
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;
|
||||
|
||||
if (!_mavlink->get_client_source_initialized()) {
|
||||
if (!_mavlink.get_client_source_initialized()) {
|
||||
|
||||
// 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
|
||||
// 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))
|
||||
|| (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_port = srcaddr.sin_port;
|
||||
|
||||
_mavlink->set_client_source_initialized();
|
||||
_mavlink.set_client_source_initialized();
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
/* if read failed, this loop won't execute */
|
||||
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. */
|
||||
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 */
|
||||
_mavlink->set_proto_version(2);
|
||||
_mavlink.set_proto_version(2);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
if (_message_statistics_enabled) {
|
||||
@@ -3179,9 +3179,9 @@ MavlinkReceiver::run()
|
||||
|
||||
/* count received bytes (nread will be -1 on read error) */
|
||||
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_lost_count = _total_lost_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.send();
|
||||
|
||||
if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) {
|
||||
if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) {
|
||||
_parameters_manager.send();
|
||||
}
|
||||
|
||||
if (_mavlink->ftp_enabled()) {
|
||||
if (_mavlink.ftp_enabled()) {
|
||||
_mavlink_ftp.send();
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user