9247876068

Committed 27 May 2024 01:35AM UTC coverage: 37.127% (+0.1%) from 37.012%

Build # 9247876068

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2312 from mavlink/pr-cookies

Use uint64_t instead of void * as cookies

Run Details

120 of 207 new or added lines in 25 files covered. (57.97%)

6 existing lines in 3 files now uncovered.

10931 of 29442 relevant lines covered (37.13%)

120.7 hits per line

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

14.29

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

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

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

mavlink / MAVSDK / 9247876068

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