10650666993

Committed 01 Sep 2024 03:31AM UTC coverage: 38.063% (+0.3%) from 37.783%

Build # 10650666993

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2381 from mavlink/pr-request-old-messages

Switch to new REQUEST_MESSAGE but fall back to deprecated request message commands if required

Run Details

137 of 262 new or added lines in 10 files covered. (52.29%)

16 existing lines in 7 files now uncovered.

11450 of 30082 relevant lines covered (38.06%)

261.09 hits per line

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

13.93

/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 _server_component_impl->make_command_ack_message(
                                command, MAV_RESULT::MAV_RESULT_ACCEPTED);
                        } else {
                            return _server_component_impl->make_command_ack_message(
                                command, MAV_RESULT::MAV_RESULT_FAILED);
                        }
                    } else {
                        return _server_component_impl->make_command_ack_message(
                            command, MAV_RESULT::MAV_RESULT_DENIED);
                    }

                default:
                    return _server_component_impl->make_command_ack_message(
                        command, MAV_RESULT::MAV_RESULT_DENIED);
            }
        },
        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;
}

} // 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()	1✔
16	{
17	_server_component_impl->unregister_plugin(this);	1✔
18	std::lock_guard<std::mutex> lock(_mutex);	2✔
19	for (const auto& request : _interval_requests) {	1✔
20	_server_component_impl->remove_call_every(request.cookie);	×
21	}
22	}	1✔
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) {	3✔
79	std::lock_guard<std::mutex> lock(_mutex);	2✔
80	uint32_t msgid = static_cast<uint32_t>(command.params.param1);	1✔
81
82	switch (msgid) {	1✔
UNCOV 83	case MAVLINK_MSG_ID_HOME_POSITION:	×
UNCOV 84	if (_maybe_home) {	×
85	if (_send_home()) {	×
86	return _server_component_impl->make_command_ack_message(	×
87	command, MAV_RESULT::MAV_RESULT_ACCEPTED);	×
88	} else {
89	return _server_component_impl->make_command_ack_message(	×
90	command, MAV_RESULT::MAV_RESULT_FAILED);	×
91	}
92	} else {
UNCOV 93	return _server_component_impl->make_command_ack_message(	×
UNCOV 94	command, MAV_RESULT::MAV_RESULT_DENIED);	×
95	}
96
97	default:	1✔
98	return _server_component_impl->make_command_ack_message(	1✔
99	command, MAV_RESULT::MAV_RESULT_DENIED);	1✔
100	}
101	},
102	this);
103	}	1✔
104
105	void TelemetryServerImpl::deinit() {}	1✔
106
107	TelemetryServer::Result TelemetryServerImpl::publish_position(	×
108	TelemetryServer::Position position,
109	TelemetryServer::VelocityNed velocity_ned,
110	TelemetryServer::Heading heading)
111	{
112	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	×
113	uint8_t channel) {
114	mavlink_message_t message;
115	mavlink_msg_global_position_int_pack_chan(	×
116	mavlink_address.system_id,	×
117	mavlink_address.component_id,	×
118	channel,
119	&message,
120	get_boot_time_ms(),	×
121	static_cast<int32_t>(position.latitude_deg * 1E7),	×
122	static_cast<int32_t>(position.longitude_deg * 1E7),	×
123	static_cast<int32_t>(static_cast<double>(position.absolute_altitude_m) * 1E3),	×
124	static_cast<int32_t>(static_cast<double>(position.relative_altitude_m) * 1E3),	×
125	static_cast<int16_t>(static_cast<double>(velocity_ned.north_m_s) * 1E2),	×
126	static_cast<int16_t>(static_cast<double>(velocity_ned.east_m_s) * 1E2),	×
127	static_cast<int16_t>(static_cast<double>(velocity_ned.down_m_s) * 1E2),	×
128	static_cast<uint16_t>(static_cast<double>(heading.heading_deg) * 1E2));	×
129	return message;	×
130	}) ?	×
131	TelemetryServer::Result::Success :
132	TelemetryServer::Result::Unsupported;	×
133	}
134
135	TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)	×
136	{
137	_maybe_home = home;	×
138
139	return _send_home() ? TelemetryServer::Result::Success :	×
140	TelemetryServer::Result::ConnectionError;	×
141	}
142
143	bool TelemetryServerImpl::_send_home()	×
144	{
145	// Requires _maybe_home to be set.
146
147	const auto home = _maybe_home.value();	×
148
149	const float q[4] = {};	×
150
151	return _server_component_impl->queue_message(	×
152	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
153	mavlink_message_t message;
154	mavlink_msg_home_position_pack_chan(	×
155	mavlink_address.system_id,	×
156	mavlink_address.component_id,	×
157	channel,
158	&message,
159	static_cast<int32_t>(home.latitude_deg * 1E7),	×
160	static_cast<int32_t>(home.longitude_deg * 1E7),	×
161	static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),	×
162	0, // Local X
163	0, // Local Y
164	0, // Local Z
165	q, // surface normal transform	×
166	NAN, // approach x
167	NAN, // approach y
168	NAN, // approach z
169	get_boot_time_ms() // TO-DO: System boot	×
170	);
171	return message;	×
172	});	×
173	}
174
175	TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(	×
176	TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
177	{
178	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	×
179	uint8_t channel) {
180	mavlink_message_t message;
181	mavlink_msg_gps_raw_int_pack_chan(	×
182	mavlink_address.system_id,	×
183	mavlink_address.component_id,	×
184	channel,
185	&message,
186	raw_gps.timestamp_us,	×
187	static_cast<uint8_t>(gps_info.fix_type),	×
188	static_cast<int32_t>(raw_gps.latitude_deg * 1E7),	×
189	static_cast<int32_t>(raw_gps.longitude_deg * 1E7),	×
190	static_cast<int32_t>(static_cast<double>(raw_gps.absolute_altitude_m) * 1E3),	×
191	static_cast<uint16_t>(static_cast<double>(raw_gps.hdop) * 1E2),	×
192	static_cast<uint16_t>(static_cast<double>(raw_gps.vdop) * 1E2),	×
193	static_cast<uint16_t>(static_cast<double>(raw_gps.velocity_m_s) * 1E2),	×
194	static_cast<uint16_t>(static_cast<double>(raw_gps.cog_deg) * 1E2),	×
195	static_cast<uint8_t>(gps_info.num_satellites),	×
196	static_cast<int32_t>(static_cast<double>(raw_gps.altitude_ellipsoid_m) * 1E3),	×
197	static_cast<uint32_t>(static_cast<double>(raw_gps.horizontal_uncertainty_m) * 1E3),	×
198	static_cast<uint32_t>(static_cast<double>(raw_gps.vertical_uncertainty_m) * 1E3),	×
199	static_cast<uint32_t>(static_cast<double>(raw_gps.velocity_uncertainty_m_s) * 1E3),	×
200	static_cast<uint32_t>(static_cast<double>(raw_gps.heading_uncertainty_deg) * 1E5),	×
201	static_cast<uint16_t>(static_cast<double>(raw_gps.yaw_deg) * 1E2));	×
202	return message;	×
203	}) ?	×
204	TelemetryServer::Result::Success :
205	TelemetryServer::Result::Unsupported;	×
206	}
207
208	TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Battery battery)	×
209	{
210	uint16_t voltages[10] = {0};	×
211	uint16_t voltages_ext[4] = {0};	×
212	voltages[0] = static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3);	×
213
214	return _server_component_impl->queue_message(	×
215	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
216	mavlink_message_t message;
217	mavlink_msg_battery_status_pack_chan(	×
218	mavlink_address.system_id,	×
219	mavlink_address.component_id,	×
220	channel,
221	&message,
222	0,
223	MAV_BATTERY_FUNCTION_ALL,
224	MAV_BATTERY_TYPE_LIPO,
225	INT16_MAX,
226	voltages,	×
227	-1, // TODO publish all battery data
228	-1,
229	-1,
230	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
231	0,
232	MAV_BATTERY_CHARGE_STATE_UNDEFINED,
233	voltages_ext,	×
234	MAV_BATTERY_MODE_UNKNOWN,
235	0);
236	return message;	×
237	}) ?	×
238	TelemetryServer::Result::Success :
239	TelemetryServer::Result::Unsupported;	×
240	}
241
242	TelemetryServer::Result
243	TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor distance_sensor)	×
244	{
245	std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec	×
246
247	return _server_component_impl->queue_message([&](MavlinkAddress mavlink_address,	×
248	uint8_t channel) {
249	mavlink_message_t message;
250	mavlink_msg_distance_sensor_pack_chan(	×
251	mavlink_address.system_id,	×
252	mavlink_address.component_id,	×
253	channel,
254	&message,
255	0,
256	static_cast<uint16_t>(static_cast<double>(distance_sensor.minimum_distance_m) * 1e2),	×
257	static_cast<uint16_t>(static_cast<double>(distance_sensor.maximum_distance_m) * 1e2),	×
258	static_cast<uint16_t>(static_cast<double>(distance_sensor.current_distance_m) * 1e2),	×
259	MAV_DISTANCE_SENSOR_UNKNOWN,
260	0,
261	MAV_SENSOR_ROTATION_NONE,
262	255,
263	0,
264	0,
265	q.data(),	×
266	0);
267	return message;	×
268	}) ?	×
269	TelemetryServer::Result::Success :
270	TelemetryServer::Result::Unsupported;	×
271	}
272
273	TelemetryServer::Result
274	TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text)	3✔
275	{
276	int type = MAV_SEVERITY_INFO;	3✔
277	switch (status_text.type) {	3✔
278	case TelemetryServer::StatusTextType::Emergency:	×
279	type = MAV_SEVERITY_EMERGENCY;	×
280	break;	×
281	case TelemetryServer::StatusTextType::Alert:	×
282	type = MAV_SEVERITY_ALERT;	×
283	break;	×
284	case TelemetryServer::StatusTextType::Critical:	×
285	type = MAV_SEVERITY_CRITICAL;	×
286	break;	×
287	case TelemetryServer::StatusTextType::Error:	×
288	type = MAV_SEVERITY_ERROR;	×
289	break;	×
290	case TelemetryServer::StatusTextType::Warning:	×
291	type = MAV_SEVERITY_WARNING;	×
292	break;	×
293	case TelemetryServer::StatusTextType::Notice:	×
294	type = MAV_SEVERITY_NOTICE;	×
295	break;	×
296	case TelemetryServer::StatusTextType::Info:	3✔
297	type = MAV_SEVERITY_INFO;	3✔
298	break;	3✔
299	case TelemetryServer::StatusTextType::Debug:	×
300	type = MAV_SEVERITY_DEBUG;	×
301	break;	×
302	default:	×
303	LogWarn() << "Unknown StatusText severity";	×
304	type = MAV_SEVERITY_INFO;	×
305	break;	×
306	}
307
308	// Prevent memcpy in mavlink function to read outside of allocated data.
309	status_text.text.resize(sizeof(mavlink_statustext_t::text));	3✔
310
311	return _server_component_impl->queue_message(	9✔
312	[&](MavlinkAddress mavlink_address, uint8_t channel) {	3✔
313	mavlink_message_t message;
314	mavlink_msg_statustext_pack_chan(	3✔
315	mavlink_address.system_id,	3✔
316	mavlink_address.component_id,	3✔
317	channel,
318	&message,
319	type,	3✔
320	status_text.text.data(),	3✔
321	0,
322	0);
323	return message;	3✔
324	}) ?	6✔
325	TelemetryServer::Result::Success :
326	TelemetryServer::Result::Unsupported;	3✔
327	}
328
329	TelemetryServer::Result TelemetryServerImpl::publish_odometry(TelemetryServer::Odometry odometry)	×
330	{
331	UNUSED(odometry);	×
332
333	// TODO :)
334	return {};	×
335	}
336
337	TelemetryServer::Result TelemetryServerImpl::publish_position_velocity_ned(	×
338	TelemetryServer::PositionVelocityNed position_velocity_ned)
339	{
340	return _server_component_impl->queue_message(	×
341	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
342	mavlink_message_t message;
343	mavlink_msg_local_position_ned_pack_chan(	×
344	mavlink_address.system_id,	×
345	mavlink_address.component_id,	×
346	channel,
347	&message,
348	get_boot_time_ms(),	×
349	position_velocity_ned.position.north_m,	×
350	position_velocity_ned.position.east_m,
351	position_velocity_ned.position.down_m,
352	position_velocity_ned.velocity.north_m_s,
353	position_velocity_ned.velocity.east_m_s,
354	position_velocity_ned.velocity.down_m_s);
355	return message;	×
356	}) ?	×
357	TelemetryServer::Result::Success :
358	TelemetryServer::Result::Unsupported;	×
359	}
360
361	TelemetryServer::Result
362	TelemetryServerImpl::publish_ground_truth(TelemetryServer::GroundTruth ground_truth)	×
363	{
364	UNUSED(ground_truth);	×
365
366	// TODO :)
367	return {};	×
368	}
369
370	TelemetryServer::Result TelemetryServerImpl::publish_imu(TelemetryServer::Imu imu)	×
371	{
372	UNUSED(imu);	×
373
374	// TODO :)
375	return {};	×
376	}
377
378	TelemetryServer::Result TelemetryServerImpl::publish_scaled_imu(TelemetryServer::Imu imu)	×
379	{
380	UNUSED(imu);	×
381
382	// TODO :)
383	return {};	×
384	}
385
386	TelemetryServer::Result TelemetryServerImpl::publish_raw_imu(TelemetryServer::Imu imu)	×
387	{
388	UNUSED(imu);	×
389
390	// TODO :)
391	return {};	×
392	}
393
394	TelemetryServer::Result TelemetryServerImpl::publish_unix_epoch_time(uint64_t time_us)	×
395	{
396	return _server_component_impl->queue_message(	×
397	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
398	mavlink_message_t message;
399	mavlink_msg_system_time_pack_chan(	×
400	mavlink_address.system_id,	×
401	mavlink_address.component_id,	×
402	channel,
403	&message,
404	time_us,	×
405	0 // TODO: add timestamping in general
406	);
407	return message;	×
408	}) ?	×
409	TelemetryServer::Result::Success :
410	TelemetryServer::Result::Unsupported;	×
411	}
412
413	TelemetryServer::Result TelemetryServerImpl::publish_sys_status(	×
414	TelemetryServer::Battery battery,
415	bool rc_receiver_status,
416	bool gyro_status,
417	bool accel_status,
418	bool mag_status,
419	bool gps_status)
420	{
421	int32_t sensors = 0;	×
422
423	if (rc_receiver_status) {	×
424	sensors \|= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;	×
425	}
426	if (gyro_status) {	×
427	sensors \|= MAV_SYS_STATUS_SENSOR_3D_GYRO;	×
428	}
429	if (accel_status) {	×
430	sensors \|= MAV_SYS_STATUS_SENSOR_3D_ACCEL;	×
431	}
432	if (mag_status) {	×
433	sensors \|= MAV_SYS_STATUS_SENSOR_3D_MAG;	×
434	}
435	if (gps_status) {	×
436	sensors \|= MAV_SYS_STATUS_SENSOR_GPS;	×
437	}
438
439	return _server_component_impl->queue_message(	×
440	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
441	mavlink_message_t message;
442	mavlink_msg_sys_status_pack_chan(	×
443	mavlink_address.system_id,	×
444	mavlink_address.component_id,	×
445	channel,
446	&message,
447	sensors,	×
448	sensors,	×
449	sensors,	×
450	0,
451	static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3),	×
452	-1,
453	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
454	0,
455	0,
456	0,
457	0,
458	0,
459	0,
460	0,
461	0,
462	0);
463	return message;	×
464	}) ?	×
465	TelemetryServer::Result::Success :
466	TelemetryServer::Result::Unsupported;	×
467	}
468
469	uint8_t to_mav_vtol_state(TelemetryServer::VtolState vtol_state)	×
470	{
471	switch (vtol_state) {	×
472	case TelemetryServer::VtolState::Undefined:	×
473	return MAV_VTOL_STATE_UNDEFINED;	×
474	case TelemetryServer::VtolState::TransitionToFw:	×
475	return MAV_VTOL_STATE_TRANSITION_TO_FW;	×
476	case TelemetryServer::VtolState::TransitionToMc:	×
477	return MAV_VTOL_STATE_TRANSITION_TO_MC;	×
478	case TelemetryServer::VtolState::Mc:	×
479	return MAV_VTOL_STATE_MC;	×
480	case TelemetryServer::VtolState::Fw:	×
481	return MAV_VTOL_STATE_FW;	×
482	default:	×
483	return MAV_VTOL_STATE_UNDEFINED;	×
484	}
485	}
486
487	uint8_t to_mav_landed_state(TelemetryServer::LandedState landed_state)	×
488	{
489	switch (landed_state) {	×
490	case TelemetryServer::LandedState::InAir:	×
491	return MAV_LANDED_STATE_IN_AIR;	×
492	case TelemetryServer::LandedState::TakingOff:	×
493	return MAV_LANDED_STATE_TAKEOFF;	×
494	case TelemetryServer::LandedState::Landing:	×
495	return MAV_LANDED_STATE_LANDING;	×
496	case TelemetryServer::LandedState::OnGround:	×
497	return MAV_LANDED_STATE_ON_GROUND;	×
498	default:	×
499	return MAV_LANDED_STATE_UNDEFINED;	×
500	}
501	}
502
503	TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(	×
504	TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state)
505	{
506	return _server_component_impl->queue_message(	×
507	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
508	mavlink_message_t message;
509	mavlink_msg_extended_sys_state_pack_chan(	×
510	mavlink_address.system_id,	×
511	mavlink_address.component_id,	×
512	channel,
513	&message,
514	to_mav_vtol_state(vtol_state),	×
515	to_mav_landed_state(landed_state));	×
516	return message;	×
517	}) ?	×
518	TelemetryServer::Result::Success :
519	TelemetryServer::Result::Unsupported;	×
520	}
521
522	} // namespace mavsdk

mavlink / MAVSDK / 10650666993

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