13298134957

Committed 13 Feb 2025 01:04AM UTC coverage: 44.549% (-0.08%) from 44.629%

Build # 13298134957

Build Type

push

github

Committed by

web-flow

Commit Message

Telemetry and TelemetryServer improvements (#2511)

* proto: update to latest main (1.2.1)

This fixes a regression with 1.2.0 regarding extra options.

* Add attitude and expanded fixed wing metrics for TelemetryServer

* Bug fix for ranges on attitude and heading messages

* Re-order FixedwingMetrics fields for backward compatibility

* PR feedback: altitude_msl -> absolute_altitude_m

* Proto update

* proto based on main

* Proto -> main

---------

Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: Jon Reeves <jon.reeves@elroyair.com>

Run Details

0 of 53 new or added lines in 4 files covered. (0.0%)

7 existing lines in 4 files now uncovered.

14593 of 32757 relevant lines covered (44.55%)

284.58 hits per line

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

12.58

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

mavlink / MAVSDK / 13298134957

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