18289523387

Committed 06 Oct 2025 05:41PM UTC coverage: 47.597% (+0.02%) from 47.58%

Build # 18289523387

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2667 from irajkovic/fix-mission-progress-total

Fix updating mission item total

Run Details

4 of 8 new or added lines in 1 file covered. (50.0%)

23 existing lines in 7 files now uncovered.

17035 of 35790 relevant lines covered (47.6%)

452.46 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

26.28

/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp

#include "telemetry_server_impl.h"
#include "unused.h"
#include <array>
#include <algorithm>

namespace mavsdk {

TelemetryServerImpl::TelemetryServerImpl(std::shared_ptr<ServerComponent> server_component) :
    ServerPluginImplBase(server_component)
{
    _server_component_impl->register_plugin(this);
    _start_time = std::chrono::steady_clock::now();
}

TelemetryServerImpl::~TelemetryServerImpl()
{
    // Unregister command handlers BEFORE unregistering plugin to prevent use-after-free
    _server_component_impl->unregister_mavlink_command_handler(MAV_CMD_SET_MESSAGE_INTERVAL, this);
    _server_component_impl->unregister_mavlink_command_handler(MAV_CMD_REQUEST_MESSAGE, this);

    _server_component_impl->unregister_plugin(this);
    std::lock_guard<std::mutex> lock(_mutex);
    for (const auto& request : _interval_requests) {
        _server_component_impl->remove_call_every(request.cookie);
    }
}

void TelemetryServerImpl::init()
{
    // Handle SET_MESSAGE_INTERVAL
    _server_component_impl->register_mavlink_command_handler(
        MAV_CMD_SET_MESSAGE_INTERVAL,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            std::lock_guard<std::mutex> lock(_mutex);
            uint32_t msgid = static_cast<uint32_t>(command.params.param1);
            // Set interval to 1hz if 0 (default rate)
            uint32_t interval_ms =
                command.params.param2 == 0 ?
                    1000 :
                    static_cast<uint32_t>(static_cast<double>(command.params.param2) * 1E-3);
            LogDebug() << "Setting interval for msg id: " << std::to_string(msgid)
                       << " interval_ms:" << std::to_string(interval_ms);
            auto found = std::find_if(
                _interval_requests.begin(),
                _interval_requests.end(),
                [msgid](const RequestMsgInterval& item) { return item.msg_id == msgid; });

            if (found == _interval_requests.end() && command.params.param2 != -1) {
                // If not found interval already, add a new one
                _interval_requests.push_back({msgid, interval_ms});
                //_server_component_impl->add_call_every(
                //    [this, msgid]() {
                //        std::lock_guard<std::mutex> lock_interval(_mutex);
                //        if (_msg_cache.find(msgid) != _msg_cache.end()) {
                //            // Publish if callback exists :)
                //            _server_component_impl->send_message(_msg_cache.at(msgid));
                //        }
                //    },
                //    static_cast<double>(interval_ms) * 1E-3,
                //    &_interval_requests.back().cookie);
            } else {
                if (command.params.param2 == -1) {
                    // Deregister with -1 interval
                    _server_component_impl->remove_call_every(found->cookie);
                    _interval_requests.erase(found);
                } else {
                    // Update Interval
                    found->interval = interval_ms;
                    _server_component_impl->change_call_every(
                        static_cast<double>(interval_ms) * 1E-3, found->cookie);
                }
            }

            return _server_component_impl->make_command_ack_message(
                command, MAV_RESULT::MAV_RESULT_ACCEPTED);
        },
        this);

    // Handle REQUEST_MESSAGE
    _server_component_impl->register_mavlink_command_handler(
        MAV_CMD_REQUEST_MESSAGE,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            std::lock_guard<std::mutex> lock(_mutex);
            uint32_t msgid = static_cast<uint32_t>(command.params.param1);

            switch (msgid) {
                case MAVLINK_MSG_ID_HOME_POSITION:
                    if (_maybe_home) {
                        if (_send_home()) {
                            return std::optional<mavlink_command_ack_t>{
                                _server_component_impl->make_command_ack_message(
                                    command, MAV_RESULT::MAV_RESULT_ACCEPTED)};
                        } else {
                            return std::optional<mavlink_command_ack_t>{
                                _server_component_impl->make_command_ack_message(
                                    command, MAV_RESULT::MAV_RESULT_FAILED)};
                        }
                    } else {
                        return std::optional<mavlink_command_ack_t>{
                            _server_component_impl->make_command_ack_message(
                                command, MAV_RESULT::MAV_RESULT_DENIED)};
                    }

                default:
                    // Let's hope someone else answers and keep silent. In an ideal world we would
                    // explicitly deny all the ones that we ought to answer but haven't implemented
                    // yet.
                    return std::optional<mavlink_command_ack_t>{};
            }
        },
        this);
}

void TelemetryServerImpl::deinit() {}

TelemetryServer::Result TelemetryServerImpl::publish_position(
    TelemetryServer::Position position,
    TelemetryServer::VelocityNed velocity_ned,
    TelemetryServer::Heading heading)
{
    return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
                                                     uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_global_position_int_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            get_boot_time_ms(),
            static_cast<int32_t>(position.latitude_deg * 1E7),
            static_cast<int32_t>(position.longitude_deg * 1E7),
            static_cast<int32_t>(static_cast<double>(position.absolute_altitude_m) * 1E3),
            static_cast<int32_t>(static_cast<double>(position.relative_altitude_m) * 1E3),
            static_cast<int16_t>(static_cast<double>(velocity_ned.north_m_s) * 1E2),
            static_cast<int16_t>(static_cast<double>(velocity_ned.east_m_s) * 1E2),
            static_cast<int16_t>(static_cast<double>(velocity_ned.down_m_s) * 1E2),
            static_cast<uint16_t>(static_cast<double>(heading.heading_deg) * 1E2));
        return message;
    }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)
{
    _maybe_home = home;

    return _send_home() ? TelemetryServer::Result::Success :
                          TelemetryServer::Result::ConnectionError;
}

bool TelemetryServerImpl::_send_home()
{
    // Requires _maybe_home to be set.

    const auto home = _maybe_home.value();

    const float q[4] = {};

    return _server_component_impl->queue_message(
        [&](MavlinkAddress mavlink_address, uint8_t channel) {
            mavlink_message_t message;
            mavlink_msg_home_position_pack_chan(
                mavlink_address.system_id,
                mavlink_address.component_id,
                channel,
                &message,
                static_cast<int32_t>(home.latitude_deg * 1E7),
                static_cast<int32_t>(home.longitude_deg * 1E7),
                static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),
                0, // Local X
                0, // Local Y
                0, // Local Z
                q, // surface normal transform
                NAN, // approach x
                NAN, // approach y
                NAN, // approach z
                get_boot_time_ms() // TO-DO: System boot
            );
            return message;
        });
}

TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(
    TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
{
    return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
                                                     uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_gps_raw_int_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            raw_gps.timestamp_us,
            static_cast<uint8_t>(gps_info.fix_type),
            static_cast<int32_t>(raw_gps.latitude_deg * 1E7),
            static_cast<int32_t>(raw_gps.longitude_deg * 1E7),
            static_cast<int32_t>(static_cast<double>(raw_gps.absolute_altitude_m) * 1E3),
            static_cast<uint16_t>(static_cast<double>(raw_gps.hdop) * 1E2),
            static_cast<uint16_t>(static_cast<double>(raw_gps.vdop) * 1E2),
            static_cast<uint16_t>(static_cast<double>(raw_gps.velocity_m_s) * 1E2),
            static_cast<uint16_t>(static_cast<double>(raw_gps.cog_deg) * 1E2),
            static_cast<uint8_t>(gps_info.num_satellites),
            static_cast<int32_t>(static_cast<double>(raw_gps.altitude_ellipsoid_m) * 1E3),
            static_cast<uint32_t>(static_cast<double>(raw_gps.horizontal_uncertainty_m) * 1E3),
            static_cast<uint32_t>(static_cast<double>(raw_gps.vertical_uncertainty_m) * 1E3),
            static_cast<uint32_t>(static_cast<double>(raw_gps.velocity_uncertainty_m_s) * 1E3),
            static_cast<uint32_t>(static_cast<double>(raw_gps.heading_uncertainty_deg) * 1E5),
            static_cast<uint16_t>(static_cast<double>(raw_gps.yaw_deg) * 1E2));
        return message;
    }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Battery battery)
{
    uint16_t voltages[10] = {0};
    uint16_t voltages_ext[4] = {0};
    voltages[0] = static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3);

    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_battery_status_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       0,
                       MAV_BATTERY_FUNCTION_ALL,
                       MAV_BATTERY_TYPE_LIPO,
                       INT16_MAX,
                       voltages,
                       -1, // TODO publish all battery data
                       -1,
                       -1,
                       static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),
                       0,
                       MAV_BATTERY_CHARGE_STATE_UNDEFINED,
                       voltages_ext,
                       MAV_BATTERY_MODE_UNKNOWN,
                       0);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result
TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor distance_sensor)
{
    std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec

    return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,
                                                     uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_distance_sensor_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            0,
            static_cast<uint16_t>(static_cast<double>(distance_sensor.minimum_distance_m) * 1e2),
            static_cast<uint16_t>(static_cast<double>(distance_sensor.maximum_distance_m) * 1e2),
            static_cast<uint16_t>(static_cast<double>(distance_sensor.current_distance_m) * 1e2),
            MAV_DISTANCE_SENSOR_UNKNOWN,
            0,
            MAV_SENSOR_ROTATION_NONE,
            255,
            0,
            0,
            q.data(),
            0);
        return message;
    }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result
TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text)
{
    int type = MAV_SEVERITY_INFO;
    switch (status_text.type) {
        case TelemetryServer::StatusTextType::Emergency:
            type = MAV_SEVERITY_EMERGENCY;
            break;
        case TelemetryServer::StatusTextType::Alert:
            type = MAV_SEVERITY_ALERT;
            break;
        case TelemetryServer::StatusTextType::Critical:
            type = MAV_SEVERITY_CRITICAL;
            break;
        case TelemetryServer::StatusTextType::Error:
            type = MAV_SEVERITY_ERROR;
            break;
        case TelemetryServer::StatusTextType::Warning:
            type = MAV_SEVERITY_WARNING;
            break;
        case TelemetryServer::StatusTextType::Notice:
            type = MAV_SEVERITY_NOTICE;
            break;
        case TelemetryServer::StatusTextType::Info:
            type = MAV_SEVERITY_INFO;
            break;
        case TelemetryServer::StatusTextType::Debug:
            type = MAV_SEVERITY_DEBUG;
            break;
        default:
            LogWarn() << "Unknown StatusText severity";
            type = MAV_SEVERITY_INFO;
            break;
    }

    // Prevent memcpy in mavlink function to read outside of allocated data.
    status_text.text.resize(sizeof(mavlink_statustext_t::text));

    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_statustext_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       type,
                       status_text.text.data(),
                       0,
                       0);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_odometry(TelemetryServer::Odometry odometry)
{
    UNUSED(odometry);

    // TODO :)
    return {};
}

TelemetryServer::Result TelemetryServerImpl::publish_position_velocity_ned(
    TelemetryServer::PositionVelocityNed position_velocity_ned)
{
    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_local_position_ned_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       get_boot_time_ms(),
                       position_velocity_ned.position.north_m,
                       position_velocity_ned.position.east_m,
                       position_velocity_ned.position.down_m,
                       position_velocity_ned.velocity.north_m_s,
                       position_velocity_ned.velocity.east_m_s,
                       position_velocity_ned.velocity.down_m_s);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result
TelemetryServerImpl::publish_ground_truth(TelemetryServer::GroundTruth ground_truth)
{
    UNUSED(ground_truth);

    // TODO :)
    return {};
}

TelemetryServer::Result TelemetryServerImpl::publish_imu(TelemetryServer::Imu imu)
{
    UNUSED(imu);

    // TODO :)
    return {};
}

TelemetryServer::Result TelemetryServerImpl::publish_scaled_imu(TelemetryServer::Imu imu)
{
    UNUSED(imu);

    // TODO :)
    return {};
}

TelemetryServer::Result TelemetryServerImpl::publish_raw_imu(TelemetryServer::Imu imu)
{
    UNUSED(imu);

    // TODO :)
    return {};
}

TelemetryServer::Result TelemetryServerImpl::publish_unix_epoch_time(uint64_t time_us)
{
    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_system_time_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       time_us,
                       0 // TODO: add timestamping in general
                   );
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_sys_status(
    TelemetryServer::Battery battery,
    bool rc_receiver_status,
    bool gyro_status,
    bool accel_status,
    bool mag_status,
    bool gps_status)
{
    int32_t sensors = 0;

    if (rc_receiver_status) {
        sensors |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
    }
    if (gyro_status) {
        sensors |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
    }
    if (accel_status) {
        sensors |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
    }
    if (mag_status) {
        sensors |= MAV_SYS_STATUS_SENSOR_3D_MAG;
    }
    if (gps_status) {
        sensors |= MAV_SYS_STATUS_SENSOR_GPS;
    }

    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_sys_status_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       sensors,
                       sensors,
                       sensors,
                       0,
                       static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3),
                       -1,
                       static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),
                       0,
                       0,
                       0,
                       0,
                       0,
                       0,
                       0,
                       0,
                       0);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

uint8_t to_mav_vtol_state(TelemetryServer::VtolState vtol_state)
{
    switch (vtol_state) {
        case TelemetryServer::VtolState::Undefined:
            return MAV_VTOL_STATE_UNDEFINED;
        case TelemetryServer::VtolState::TransitionToFw:
            return MAV_VTOL_STATE_TRANSITION_TO_FW;
        case TelemetryServer::VtolState::TransitionToMc:
            return MAV_VTOL_STATE_TRANSITION_TO_MC;
        case TelemetryServer::VtolState::Mc:
            return MAV_VTOL_STATE_MC;
        case TelemetryServer::VtolState::Fw:
            return MAV_VTOL_STATE_FW;
        default:
            return MAV_VTOL_STATE_UNDEFINED;
    }
}

uint8_t to_mav_landed_state(TelemetryServer::LandedState landed_state)
{
    switch (landed_state) {
        case TelemetryServer::LandedState::InAir:
            return MAV_LANDED_STATE_IN_AIR;
        case TelemetryServer::LandedState::TakingOff:
            return MAV_LANDED_STATE_TAKEOFF;
        case TelemetryServer::LandedState::Landing:
            return MAV_LANDED_STATE_LANDING;
        case TelemetryServer::LandedState::OnGround:
            return MAV_LANDED_STATE_ON_GROUND;
        default:
            return MAV_LANDED_STATE_UNDEFINED;
    }
}

TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(
    TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state)
{
    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_extended_sys_state_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       to_mav_vtol_state(vtol_state),
                       to_mav_landed_state(landed_state));
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_attitude(
    TelemetryServer::EulerAngle attitude, TelemetryServer::AngularVelocityBody angular_velocity)
{
    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_attitude_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       static_cast<uint32_t>(attitude.timestamp_us / 1000.F),
                       attitude.roll_deg * M_PI / 180.F,
                       attitude.pitch_deg * M_PI / 180.F,
                       attitude.yaw_deg * M_PI / 180.F,
                       angular_velocity.roll_rad_s,
                       angular_velocity.pitch_rad_s,
                       angular_velocity.yaw_rad_s);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_visual_flight_rules_hud(
    TelemetryServer::FixedwingMetrics fixed_wing_metrics)
{
    return _server_component_impl->queue_message(
               [&](MavlinkAddress mavlink_address, uint8_t channel) {
                   mavlink_message_t message;
                   mavlink_msg_vfr_hud_pack_chan(
                       mavlink_address.system_id,
                       mavlink_address.component_id,
                       channel,
                       &message,
                       fixed_wing_metrics.airspeed_m_s,
                       fixed_wing_metrics.groundspeed_m_s,
                       static_cast<uint16_t>(std::round(fixed_wing_metrics.heading_deg)),
                       static_cast<uint16_t>(std::round(fixed_wing_metrics.throttle_percentage)),
                       fixed_wing_metrics.absolute_altitude_m,
                       fixed_wing_metrics.climb_rate_m_s);
                   return message;
               }) ?
               TelemetryServer::Result::Success :
               TelemetryServer::Result::Unsupported;
}

} // namespace mavsdk

1	#include "telemetry_server_impl.h"
2	#include "unused.h"
3	#include <array>
4	#include <algorithm>
5
6	namespace mavsdk {
7
8	TelemetryServerImpl::TelemetryServerImpl(std::shared_ptr<ServerComponent> server_component) :	5✔
9	ServerPluginImplBase(server_component)	5✔
10	{
11	_server_component_impl->register_plugin(this);	5✔
12	_start_time = std::chrono::steady_clock::now();	5✔
13	}	5✔
14
15	TelemetryServerImpl::~TelemetryServerImpl()	10✔
16	{
17	// Unregister command handlers BEFORE unregistering plugin to prevent use-after-free
18	_server_component_impl->unregister_mavlink_command_handler(MAV_CMD_SET_MESSAGE_INTERVAL, this);	5✔
19	_server_component_impl->unregister_mavlink_command_handler(MAV_CMD_REQUEST_MESSAGE, this);	5✔
20
21	_server_component_impl->unregister_plugin(this);	5✔
22	std::lock_guard<std::mutex> lock(_mutex);	5✔
23	for (const auto& request : _interval_requests) {	5✔
24	_server_component_impl->remove_call_every(request.cookie);	×
25	}
26	}	10✔
27
28	void TelemetryServerImpl::init()	5✔
29	{
30	// Handle SET_MESSAGE_INTERVAL
31	_server_component_impl->register_mavlink_command_handler(	5✔
32	MAV_CMD_SET_MESSAGE_INTERVAL,
33	[this](const MavlinkCommandReceiver::CommandLong& command) {	×
34	std::lock_guard<std::mutex> lock(_mutex);	×
35	uint32_t msgid = static_cast<uint32_t>(command.params.param1);	×
36	// Set interval to 1hz if 0 (default rate)
37	uint32_t interval_ms =	×
38	command.params.param2 == 0 ?	×
39	1000 :
40	static_cast<uint32_t>(static_cast<double>(command.params.param2) * 1E-3);	×
41	LogDebug() << "Setting interval for msg id: " << std::to_string(msgid)	×
42	<< " interval_ms:" << std::to_string(interval_ms);	×
43	auto found = std::find_if(	×
44	_interval_requests.begin(),
45	_interval_requests.end(),
46	[msgid](const RequestMsgInterval& item) { return item.msg_id == msgid; });	×
47
48	if (found == _interval_requests.end() && command.params.param2 != -1) {	×
49	// If not found interval already, add a new one
50	_interval_requests.push_back({msgid, interval_ms});	×
51	//_server_component_impl->add_call_every(
52	// [this, msgid]() {
53	// std::lock_guard<std::mutex> lock_interval(_mutex);
54	// if (_msg_cache.find(msgid) != _msg_cache.end()) {
55	// // Publish if callback exists :)
56	// _server_component_impl->send_message(_msg_cache.at(msgid));
57	// }
58	// },
59	// static_cast<double>(interval_ms) * 1E-3,
60	// &_interval_requests.back().cookie);
61	} else {
62	if (command.params.param2 == -1) {	×
63	// Deregister with -1 interval
64	_server_component_impl->remove_call_every(found->cookie);	×
65	_interval_requests.erase(found);	×
66	} else {
67	// Update Interval
68	found->interval = interval_ms;	×
69	_server_component_impl->change_call_every(	×
70	static_cast<double>(interval_ms) * 1E-3, found->cookie);	×
71	}
72	}
73
74	return _server_component_impl->make_command_ack_message(	×
75	command, MAV_RESULT::MAV_RESULT_ACCEPTED);	×
76	},	×
77	this);
78
79	// Handle REQUEST_MESSAGE
80	_server_component_impl->register_mavlink_command_handler(	5✔
81	MAV_CMD_REQUEST_MESSAGE,
82	[this](const MavlinkCommandReceiver::CommandLong& command) {	4✔
83	std::lock_guard<std::mutex> lock(_mutex);	4✔
84	uint32_t msgid = static_cast<uint32_t>(command.params.param1);	4✔
85
86	switch (msgid) {	4✔
UNCOV 87	case MAVLINK_MSG_ID_HOME_POSITION:	×
UNCOV 88	if (_maybe_home) {	×
89	if (_send_home()) {	×
90	return std::optional<mavlink_command_ack_t>{	×
91	_server_component_impl->make_command_ack_message(	×
92	command, MAV_RESULT::MAV_RESULT_ACCEPTED)};	×
93	} else {
94	return std::optional<mavlink_command_ack_t>{	×
95	_server_component_impl->make_command_ack_message(	×
96	command, MAV_RESULT::MAV_RESULT_FAILED)};	×
97	}
98	} else {
UNCOV 99	return std::optional<mavlink_command_ack_t>{	×
UNCOV 100	_server_component_impl->make_command_ack_message(	×
UNCOV 101	command, MAV_RESULT::MAV_RESULT_DENIED)};	×
102	}
103
104	default:	4✔
105	// Let's hope someone else answers and keep silent. In an ideal world we would
106	// explicitly deny all the ones that we ought to answer but haven't implemented
107	// yet.
108	return std::optional<mavlink_command_ack_t>{};	4✔
109	}
110	},	4✔
111	this);
112	}	5✔
113
114	void TelemetryServerImpl::deinit() {}	5✔
115
116	TelemetryServer::Result TelemetryServerImpl::publish_position(	3✔
117	TelemetryServer::Position position,
118	TelemetryServer::VelocityNed velocity_ned,
119	TelemetryServer::Heading heading)
120	{
121	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	6✔
122	uint8_t channel) {
123	mavlink_message_t message;
124	mavlink_msg_global_position_int_pack_chan(	6✔
125	mavlink_address.system_id,	3✔
126	mavlink_address.component_id,	3✔
127	channel,
128	&message,
129	get_boot_time_ms(),	3✔
130	static_cast<int32_t>(position.latitude_deg * 1E7),	3✔
131	static_cast<int32_t>(position.longitude_deg * 1E7),	3✔
132	static_cast<int32_t>(static_cast<double>(position.absolute_altitude_m) * 1E3),	3✔
133	static_cast<int32_t>(static_cast<double>(position.relative_altitude_m) * 1E3),	3✔
134	static_cast<int16_t>(static_cast<double>(velocity_ned.north_m_s) * 1E2),	3✔
135	static_cast<int16_t>(static_cast<double>(velocity_ned.east_m_s) * 1E2),	3✔
136	static_cast<int16_t>(static_cast<double>(velocity_ned.down_m_s) * 1E2),	3✔
137	static_cast<uint16_t>(static_cast<double>(heading.heading_deg) * 1E2));	3✔
138	return message;	3✔
139	}) ?	3✔
140	TelemetryServer::Result::Success :
141	TelemetryServer::Result::Unsupported;	3✔
142	}
143
144	TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)	×
145	{
146	_maybe_home = home;	×
147
148	return _send_home() ? TelemetryServer::Result::Success :	×
149	TelemetryServer::Result::ConnectionError;	×
150	}
151
152	bool TelemetryServerImpl::_send_home()	×
153	{
154	// Requires _maybe_home to be set.
155
156	const auto home = _maybe_home.value();	×
157
158	const float q[4] = {};	×
159
160	return _server_component_impl->queue_message(	×
161	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
162	mavlink_message_t message;
163	mavlink_msg_home_position_pack_chan(	×
164	mavlink_address.system_id,	×
165	mavlink_address.component_id,	×
166	channel,
167	&message,
168	static_cast<int32_t>(home.latitude_deg * 1E7),	×
169	static_cast<int32_t>(home.longitude_deg * 1E7),	×
170	static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),	×
171	0, // Local X
172	0, // Local Y
173	0, // Local Z
174	q, // surface normal transform	×
175	NAN, // approach x
176	NAN, // approach y
177	NAN, // approach z
178	get_boot_time_ms() // TO-DO: System boot	×
179	);
180	return message;	×
181	});	×
182	}
183
184	TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(	1✔
185	TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
186	{
187	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	2✔
188	uint8_t channel) {
189	mavlink_message_t message;
190	mavlink_msg_gps_raw_int_pack_chan(	1✔
191	mavlink_address.system_id,	1✔
192	mavlink_address.component_id,	1✔
193	channel,
194	&message,
195	raw_gps.timestamp_us,	1✔
196	static_cast<uint8_t>(gps_info.fix_type),	1✔
197	static_cast<int32_t>(raw_gps.latitude_deg * 1E7),	1✔
198	static_cast<int32_t>(raw_gps.longitude_deg * 1E7),	1✔
199	static_cast<int32_t>(static_cast<double>(raw_gps.absolute_altitude_m) * 1E3),	1✔
200	static_cast<uint16_t>(static_cast<double>(raw_gps.hdop) * 1E2),	1✔
201	static_cast<uint16_t>(static_cast<double>(raw_gps.vdop) * 1E2),	1✔
202	static_cast<uint16_t>(static_cast<double>(raw_gps.velocity_m_s) * 1E2),	1✔
203	static_cast<uint16_t>(static_cast<double>(raw_gps.cog_deg) * 1E2),	1✔
204	static_cast<uint8_t>(gps_info.num_satellites),	1✔
205	static_cast<int32_t>(static_cast<double>(raw_gps.altitude_ellipsoid_m) * 1E3),	1✔
206	static_cast<uint32_t>(static_cast<double>(raw_gps.horizontal_uncertainty_m) * 1E3),	1✔
207	static_cast<uint32_t>(static_cast<double>(raw_gps.vertical_uncertainty_m) * 1E3),	1✔
208	static_cast<uint32_t>(static_cast<double>(raw_gps.velocity_uncertainty_m_s) * 1E3),	1✔
209	static_cast<uint32_t>(static_cast<double>(raw_gps.heading_uncertainty_deg) * 1E5),	1✔
210	static_cast<uint16_t>(static_cast<double>(raw_gps.yaw_deg) * 1E2));	1✔
211	return message;	1✔
212	}) ?	1✔
213	TelemetryServer::Result::Success :
214	TelemetryServer::Result::Unsupported;	1✔
215	}
216
217	TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Battery battery)	×
218	{
219	uint16_t voltages[10] = {0};	×
220	uint16_t voltages_ext[4] = {0};	×
221	voltages[0] = static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3);	×
222
223	return _server_component_impl->queue_message(	×
224	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
225	mavlink_message_t message;
226	mavlink_msg_battery_status_pack_chan(	×
227	mavlink_address.system_id,	×
228	mavlink_address.component_id,	×
229	channel,
230	&message,
231	0,
232	MAV_BATTERY_FUNCTION_ALL,
233	MAV_BATTERY_TYPE_LIPO,
234	INT16_MAX,
235	voltages,	×
236	-1, // TODO publish all battery data
237	-1,
238	-1,
239	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
240	0,
241	MAV_BATTERY_CHARGE_STATE_UNDEFINED,
242	voltages_ext,	×
243	MAV_BATTERY_MODE_UNKNOWN,
244	0);
245	return message;	×
246	}) ?	×
247	TelemetryServer::Result::Success :
248	TelemetryServer::Result::Unsupported;	×
249	}
250
251	TelemetryServer::Result
252	TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor distance_sensor)	×
253	{
254	std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec	×
255
256	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	×
257	uint8_t channel) {
258	mavlink_message_t message;
259	mavlink_msg_distance_sensor_pack_chan(	×
260	mavlink_address.system_id,	×
261	mavlink_address.component_id,	×
262	channel,
263	&message,
264	0,
265	static_cast<uint16_t>(static_cast<double>(distance_sensor.minimum_distance_m) * 1e2),	×
266	static_cast<uint16_t>(static_cast<double>(distance_sensor.maximum_distance_m) * 1e2),	×
267	static_cast<uint16_t>(static_cast<double>(distance_sensor.current_distance_m) * 1e2),	×
268	MAV_DISTANCE_SENSOR_UNKNOWN,
269	0,
270	MAV_SENSOR_ROTATION_NONE,
271	255,
272	0,
273	0,
274	q.data(),	×
275	0);
276	return message;	×
277	}) ?	×
278	TelemetryServer::Result::Success :
279	TelemetryServer::Result::Unsupported;	×
280	}
281
282	TelemetryServer::Result
283	TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text)	3✔
284	{
285	int type = MAV_SEVERITY_INFO;	3✔
286	switch (status_text.type) {	3✔
287	case TelemetryServer::StatusTextType::Emergency:	×
288	type = MAV_SEVERITY_EMERGENCY;	×
289	break;	×
290	case TelemetryServer::StatusTextType::Alert:	×
291	type = MAV_SEVERITY_ALERT;	×
292	break;	×
293	case TelemetryServer::StatusTextType::Critical:	×
294	type = MAV_SEVERITY_CRITICAL;	×
295	break;	×
296	case TelemetryServer::StatusTextType::Error:	×
297	type = MAV_SEVERITY_ERROR;	×
298	break;	×
299	case TelemetryServer::StatusTextType::Warning:	×
300	type = MAV_SEVERITY_WARNING;	×
301	break;	×
302	case TelemetryServer::StatusTextType::Notice:	×
303	type = MAV_SEVERITY_NOTICE;	×
304	break;	×
305	case TelemetryServer::StatusTextType::Info:	3✔
306	type = MAV_SEVERITY_INFO;	3✔
307	break;	3✔
308	case TelemetryServer::StatusTextType::Debug:	×
309	type = MAV_SEVERITY_DEBUG;	×
310	break;	×
311	default:	×
312	LogWarn() << "Unknown StatusText severity";	×
313	type = MAV_SEVERITY_INFO;	×
314	break;	×
315	}
316
317	// Prevent memcpy in mavlink function to read outside of allocated data.
318	status_text.text.resize(sizeof(mavlink_statustext_t::text));	3✔
319
320	return _server_component_impl->queue_message(	6✔
321	[&](MavlinkAddress mavlink_address, uint8_t channel) {	3✔
322	mavlink_message_t message;
323	mavlink_msg_statustext_pack_chan(	3✔
324	mavlink_address.system_id,	3✔
325	mavlink_address.component_id,	3✔
326	channel,
327	&message,
328	type,	3✔
329	status_text.text.data(),	3✔
330	0,
331	0);
332	return message;	3✔
333	}) ?	3✔
334	TelemetryServer::Result::Success :
335	TelemetryServer::Result::Unsupported;	3✔
336	}
337
338	TelemetryServer::Result TelemetryServerImpl::publish_odometry(TelemetryServer::Odometry odometry)	×
339	{
340	UNUSED(odometry);	×
341
342	// TODO :)
343	return {};	×
344	}
345
346	TelemetryServer::Result TelemetryServerImpl::publish_position_velocity_ned(	×
347	TelemetryServer::PositionVelocityNed position_velocity_ned)
348	{
349	return _server_component_impl->queue_message(	×
350	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
351	mavlink_message_t message;
352	mavlink_msg_local_position_ned_pack_chan(	×
353	mavlink_address.system_id,	×
354	mavlink_address.component_id,	×
355	channel,
356	&message,
357	get_boot_time_ms(),	×
358	position_velocity_ned.position.north_m,	×
359	position_velocity_ned.position.east_m,
360	position_velocity_ned.position.down_m,
361	position_velocity_ned.velocity.north_m_s,
362	position_velocity_ned.velocity.east_m_s,
363	position_velocity_ned.velocity.down_m_s);
364	return message;	×
365	}) ?	×
366	TelemetryServer::Result::Success :
367	TelemetryServer::Result::Unsupported;	×
368	}
369
370	TelemetryServer::Result
371	TelemetryServerImpl::publish_ground_truth(TelemetryServer::GroundTruth ground_truth)	×
372	{
373	UNUSED(ground_truth);	×
374
375	// TODO :)
376	return {};	×
377	}
378
379	TelemetryServer::Result TelemetryServerImpl::publish_imu(TelemetryServer::Imu imu)	×
380	{
381	UNUSED(imu);	×
382
383	// TODO :)
384	return {};	×
385	}
386
387	TelemetryServer::Result TelemetryServerImpl::publish_scaled_imu(TelemetryServer::Imu imu)	×
388	{
389	UNUSED(imu);	×
390
391	// TODO :)
392	return {};	×
393	}
394
395	TelemetryServer::Result TelemetryServerImpl::publish_raw_imu(TelemetryServer::Imu imu)	×
396	{
397	UNUSED(imu);	×
398
399	// TODO :)
400	return {};	×
401	}
402
403	TelemetryServer::Result TelemetryServerImpl::publish_unix_epoch_time(uint64_t time_us)	×
404	{
405	return _server_component_impl->queue_message(	×
406	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
407	mavlink_message_t message;
408	mavlink_msg_system_time_pack_chan(	×
409	mavlink_address.system_id,	×
410	mavlink_address.component_id,	×
411	channel,
412	&message,
413	time_us,	×
414	0 // TODO: add timestamping in general
415	);
416	return message;	×
417	}) ?	×
418	TelemetryServer::Result::Success :
419	TelemetryServer::Result::Unsupported;	×
420	}
421
422	TelemetryServer::Result TelemetryServerImpl::publish_sys_status(	×
423	TelemetryServer::Battery battery,
424	bool rc_receiver_status,
425	bool gyro_status,
426	bool accel_status,
427	bool mag_status,
428	bool gps_status)
429	{
430	int32_t sensors = 0;	×
431
432	if (rc_receiver_status) {	×
433	sensors \|= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;	×
434	}
435	if (gyro_status) {	×
436	sensors \|= MAV_SYS_STATUS_SENSOR_3D_GYRO;	×
437	}
438	if (accel_status) {	×
439	sensors \|= MAV_SYS_STATUS_SENSOR_3D_ACCEL;	×
440	}
441	if (mag_status) {	×
442	sensors \|= MAV_SYS_STATUS_SENSOR_3D_MAG;	×
443	}
444	if (gps_status) {	×
445	sensors \|= MAV_SYS_STATUS_SENSOR_GPS;	×
446	}
447
448	return _server_component_impl->queue_message(	×
449	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
450	mavlink_message_t message;
451	mavlink_msg_sys_status_pack_chan(	×
452	mavlink_address.system_id,	×
453	mavlink_address.component_id,	×
454	channel,
455	&message,
456	sensors,	×
457	sensors,	×
458	sensors,	×
459	0,
460	static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3),	×
461	-1,
462	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
463	0,
464	0,
465	0,
466	0,
467	0,
468	0,
469	0,
470	0,
471	0);
472	return message;	×
473	}) ?	×
474	TelemetryServer::Result::Success :
475	TelemetryServer::Result::Unsupported;	×
476	}
477
478	uint8_t to_mav_vtol_state(TelemetryServer::VtolState vtol_state)	×
479	{
480	switch (vtol_state) {	×
481	case TelemetryServer::VtolState::Undefined:	×
482	return MAV_VTOL_STATE_UNDEFINED;	×
483	case TelemetryServer::VtolState::TransitionToFw:	×
484	return MAV_VTOL_STATE_TRANSITION_TO_FW;	×
485	case TelemetryServer::VtolState::TransitionToMc:	×
486	return MAV_VTOL_STATE_TRANSITION_TO_MC;	×
487	case TelemetryServer::VtolState::Mc:	×
488	return MAV_VTOL_STATE_MC;	×
489	case TelemetryServer::VtolState::Fw:	×
490	return MAV_VTOL_STATE_FW;	×
491	default:	×
492	return MAV_VTOL_STATE_UNDEFINED;	×
493	}
494	}
495
496	uint8_t to_mav_landed_state(TelemetryServer::LandedState landed_state)	×
497	{
498	switch (landed_state) {	×
499	case TelemetryServer::LandedState::InAir:	×
500	return MAV_LANDED_STATE_IN_AIR;	×
501	case TelemetryServer::LandedState::TakingOff:	×
502	return MAV_LANDED_STATE_TAKEOFF;	×
503	case TelemetryServer::LandedState::Landing:	×
504	return MAV_LANDED_STATE_LANDING;	×
505	case TelemetryServer::LandedState::OnGround:	×
506	return MAV_LANDED_STATE_ON_GROUND;	×
507	default:	×
508	return MAV_LANDED_STATE_UNDEFINED;	×
509	}
510	}
511
512	TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(	×
513	TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state)
514	{
515	return _server_component_impl->queue_message(	×
516	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
517	mavlink_message_t message;
518	mavlink_msg_extended_sys_state_pack_chan(	×
519	mavlink_address.system_id,	×
520	mavlink_address.component_id,	×
521	channel,
522	&message,
523	to_mav_vtol_state(vtol_state),	×
524	to_mav_landed_state(landed_state));	×
525	return message;	×
526	}) ?	×
527	TelemetryServer::Result::Success :
528	TelemetryServer::Result::Unsupported;	×
529	}
530
531	TelemetryServer::Result TelemetryServerImpl::publish_attitude(	×
532	TelemetryServer::EulerAngle attitude, TelemetryServer::AngularVelocityBody angular_velocity)
533	{
534	return _server_component_impl->queue_message(	×
535	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
536	mavlink_message_t message;
537	mavlink_msg_attitude_pack_chan(	×
538	mavlink_address.system_id,	×
539	mavlink_address.component_id,	×
540	channel,
541	&message,
542	static_cast<uint32_t>(attitude.timestamp_us / 1000.F),	×
543	attitude.roll_deg * M_PI / 180.F,	×
544	attitude.pitch_deg * M_PI / 180.F,	×
545	attitude.yaw_deg * M_PI / 180.F,	×
546	angular_velocity.roll_rad_s,	×
547	angular_velocity.pitch_rad_s,
548	angular_velocity.yaw_rad_s);
549	return message;	×
550	}) ?	×
551	TelemetryServer::Result::Success :
552	TelemetryServer::Result::Unsupported;	×
553	}
554
555	TelemetryServer::Result TelemetryServerImpl::publish_visual_flight_rules_hud(	×
556	TelemetryServer::FixedwingMetrics fixed_wing_metrics)
557	{
558	return _server_component_impl->queue_message(	×
559	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
560	mavlink_message_t message;
561	mavlink_msg_vfr_hud_pack_chan(	×
562	mavlink_address.system_id,	×
563	mavlink_address.component_id,	×
564	channel,
565	&message,
566	fixed_wing_metrics.airspeed_m_s,	×
567	fixed_wing_metrics.groundspeed_m_s,
568	static_cast<uint16_t>(std::round(fixed_wing_metrics.heading_deg)),	×
569	static_cast<uint16_t>(std::round(fixed_wing_metrics.throttle_percentage)),	×
570	fixed_wing_metrics.absolute_altitude_m,
571	fixed_wing_metrics.climb_rate_m_s);
572	return message;	×
573	}) ?	×
574	TelemetryServer::Result::Success :
575	TelemetryServer::Result::Unsupported;	×
576	}
577
578	} // namespace mavsdk

mavlink / MAVSDK / 18289523387

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous