4053943449

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #1964 from akkawimo/potaito/distance-sensor-server

Run Details

17 of 17 new or added lines in 3 files covered. (100.0%)

7418 of 23664 relevant lines covered (31.35%)

22.06 hits per line

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

11.34

/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::unique_lock<std::mutex> lock(_interval_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(_interval_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, nullptr});
                _server_component_impl->add_call_every(
                    [this, msgid]() {
                        std::lock_guard<std::mutex> lock_interval(_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);
}

void TelemetryServerImpl::deinit() {}

TelemetryServer::Result TelemetryServerImpl::publish_position(
    TelemetryServer::Position position,
    TelemetryServer::VelocityNed velocity_ned,
    TelemetryServer::Heading heading)
{
    mavlink_message_t msg;
    mavlink_msg_global_position_int_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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));

    add_msg_cache(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)
{
    mavlink_message_t msg;
    const float q[4] = {};
    mavlink_msg_home_position_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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
    );

    add_msg_cache(MAVLINK_MSG_ID_HOME_POSITION, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(
    TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
{
    mavlink_message_t msg;
    mavlink_msg_gps_raw_int_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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));

    add_msg_cache(MAVLINK_MSG_ID_GPS_RAW_INT, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Battery battery)
{
    mavlink_message_t msg;

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

    mavlink_msg_battery_status_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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);

    add_msg_cache(MAVLINK_MSG_ID_BATTERY_STATUS, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result
TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor distance_sensor)
{
    mavlink_message_t msg;

    std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec

    mavlink_msg_distance_sensor_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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);

    add_msg_cache(MAVLINK_MSG_ID_DISTANCE_SENSOR, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result
TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text)
{
    mavlink_message_t msg;

    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));

    mavlink_msg_statustext_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        type,
        status_text.text.data(),
        0,
        0);

    return _server_component_impl->send_message(msg) ? 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)
{
    mavlink_message_t msg;
    mavlink_msg_local_position_ned_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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);

    add_msg_cache(MAVLINK_MSG_ID_LOCAL_POSITION_NED, msg);

    return _server_component_impl->send_message(msg) ? 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)
{
    UNUSED(time_us);

    // TODO :)
    return {};
}

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;
    }
    mavlink_message_t msg;
    mavlink_msg_sys_status_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        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);

    add_msg_cache(MAVLINK_MSG_ID_SYS_STATUS, msg);

    return _server_component_impl->send_message(msg) ? 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)
{
    mavlink_message_t msg;
    mavlink_msg_extended_sys_state_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &msg,
        to_mav_vtol_state(vtol_state),
        to_mav_landed_state(landed_state));

    add_msg_cache(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, msg);

    return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :
                                                       TelemetryServer::Result::Unsupported;
}

void TelemetryServerImpl::add_msg_cache(uint64_t id, mavlink_message_t& msg)
{
    std::unique_lock<std::mutex> lock(_interval_mutex);
    _msg_cache.insert_or_assign(id, msg);
}

} // 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::unique_lock<std::mutex> lock(_interval_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(_interval_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
45	_interval_requests.push_back({msgid, interval_ms, nullptr});	×
46	_server_component_impl->add_call_every(	×
47	[this, msgid]() {	×
48	std::lock_guard<std::mutex> lock_interval(_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	}	1✔
74
75	void TelemetryServerImpl::deinit() {}	1✔
76
77	TelemetryServer::Result TelemetryServerImpl::publish_position(	×
78	TelemetryServer::Position position,
79	TelemetryServer::VelocityNed velocity_ned,
80	TelemetryServer::Heading heading)
81	{
82	mavlink_message_t msg;	×
83	mavlink_msg_global_position_int_pack(	×
84	_server_component_impl->get_own_system_id(),	×
85	_server_component_impl->get_own_component_id(),	×
86	&msg,
87	get_boot_time_ms(),	×
88	static_cast<int32_t>(position.latitude_deg * 1E7),	×
89	static_cast<int32_t>(position.longitude_deg * 1E7),	×
90	static_cast<int32_t>(static_cast<double>(position.absolute_altitude_m) * 1E3),	×
91	static_cast<int32_t>(static_cast<double>(position.relative_altitude_m) * 1E3),	×
92	static_cast<int16_t>(static_cast<double>(velocity_ned.north_m_s) * 1E2),	×
93	static_cast<int16_t>(static_cast<double>(velocity_ned.east_m_s) * 1E2),	×
94	static_cast<int16_t>(static_cast<double>(velocity_ned.down_m_s) * 1E2),	×
95	static_cast<uint16_t>(static_cast<double>(heading.heading_deg) * 1E2));	×
96
97	add_msg_cache(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, msg);	×
98
99	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
100	TelemetryServer::Result::Unsupported;	×
101	}
102
103	TelemetryServer::Result TelemetryServerImpl::publish_home(TelemetryServer::Position home)	×
104	{
105	mavlink_message_t msg;	×
106	const float q[4] = {};	×
107	mavlink_msg_home_position_pack(	×
108	_server_component_impl->get_own_system_id(),	×
109	_server_component_impl->get_own_component_id(),	×
110	&msg,
111	static_cast<int32_t>(home.latitude_deg * 1E7),	×
112	static_cast<int32_t>(home.longitude_deg * 1E7),	×
113	static_cast<int32_t>(static_cast<double>(home.absolute_altitude_m) * 1E-3),	×
114	0, // Local X
115	0, // Local Y
116	0, // Local Z
117	q, // surface normal transform
118	NAN, // approach x
119	NAN, // approach y
120	NAN, // approach z
121	get_boot_time_ms() // TO-DO: System boot
122	);
123
124	add_msg_cache(MAVLINK_MSG_ID_HOME_POSITION, msg);	×
125
126	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
127	TelemetryServer::Result::Unsupported;	×
128	}
129
130	TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(	×
131	TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
132	{
133	mavlink_message_t msg;	×
134	mavlink_msg_gps_raw_int_pack(	×
135	_server_component_impl->get_own_system_id(),	×
136	_server_component_impl->get_own_component_id(),	×
137	&msg,
138	raw_gps.timestamp_us,
139	static_cast<uint8_t>(gps_info.fix_type),	×
140	static_cast<int32_t>(raw_gps.latitude_deg * 1E7),	×
141	static_cast<int32_t>(raw_gps.longitude_deg * 1E7),	×
142	static_cast<int32_t>(static_cast<double>(raw_gps.absolute_altitude_m) * 1E3),	×
143	static_cast<uint16_t>(static_cast<double>(raw_gps.hdop) * 1E2),	×
144	static_cast<uint16_t>(static_cast<double>(raw_gps.vdop) * 1E2),	×
145	static_cast<uint16_t>(static_cast<double>(raw_gps.velocity_m_s) * 1E2),	×
146	static_cast<uint16_t>(static_cast<double>(raw_gps.cog_deg) * 1E2),	×
147	static_cast<uint8_t>(gps_info.num_satellites),	×
148	static_cast<int32_t>(static_cast<double>(raw_gps.altitude_ellipsoid_m) * 1E3),	×
149	static_cast<uint32_t>(static_cast<double>(raw_gps.horizontal_uncertainty_m) * 1E3),	×
150	static_cast<uint32_t>(static_cast<double>(raw_gps.vertical_uncertainty_m) * 1E3),	×
151	static_cast<uint32_t>(static_cast<double>(raw_gps.velocity_uncertainty_m_s) * 1E3),	×
152	static_cast<uint32_t>(static_cast<double>(raw_gps.heading_uncertainty_deg) * 1E5),	×
153	static_cast<uint16_t>(static_cast<double>(raw_gps.yaw_deg) * 1E2));	×
154
155	add_msg_cache(MAVLINK_MSG_ID_GPS_RAW_INT, msg);	×
156
157	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
158	TelemetryServer::Result::Unsupported;	×
159	}
160
161	TelemetryServer::Result TelemetryServerImpl::publish_battery(TelemetryServer::Battery battery)	×
162	{
163	mavlink_message_t msg;	×
164
165	uint16_t voltages[10] = {0};	×
166	uint16_t voltages_ext[4] = {0};	×
167	voltages[0] = static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3);	×
168
169	mavlink_msg_battery_status_pack(	×
170	_server_component_impl->get_own_system_id(),	×
171	_server_component_impl->get_own_component_id(),	×
172	&msg,
173	0,
174	MAV_BATTERY_FUNCTION_ALL,
175	MAV_BATTERY_TYPE_LIPO,
176	INT16_MAX,
177	voltages,
178	-1, // TODO publish all battery data
179	-1,
180	-1,
181	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
182	0,
183	MAV_BATTERY_CHARGE_STATE_UNDEFINED,
184	voltages_ext,
185	MAV_BATTERY_MODE_UNKNOWN,
186	0);
187
188	add_msg_cache(MAVLINK_MSG_ID_BATTERY_STATUS, msg);	×
189
190	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
191	TelemetryServer::Result::Unsupported;	×
192	}
193
194	TelemetryServer::Result
195	TelemetryServerImpl::publish_distance_sensor(TelemetryServer::DistanceSensor distance_sensor)	×
196	{
197	mavlink_message_t msg;	×
198
199	std::array<float, 4> q{0}; // Invalid Quaternion per mavlink spec	×
200
201	mavlink_msg_distance_sensor_pack(	×
202	_server_component_impl->get_own_system_id(),	×
203	_server_component_impl->get_own_component_id(),	×
204	&msg,
205	0,
206	static_cast<uint16_t>(static_cast<double>(distance_sensor.minimum_distance_m) * 1e2),	×
207	static_cast<uint16_t>(static_cast<double>(distance_sensor.maximum_distance_m) * 1e2),	×
208	static_cast<uint16_t>(static_cast<double>(distance_sensor.current_distance_m) * 1e2),	×
209	MAV_DISTANCE_SENSOR_UNKNOWN,
210	0,
211	MAV_SENSOR_ROTATION_NONE,
212	255,
213	0,
214	0,
215	q.data(),	×
216	0);
217
218	add_msg_cache(MAVLINK_MSG_ID_DISTANCE_SENSOR, msg);	×
219
220	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
221	TelemetryServer::Result::Unsupported;	×
222	}
223
224	TelemetryServer::Result
225	TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text)	3✔
226	{
227	mavlink_message_t msg;	3✔
228
229	int type = MAV_SEVERITY_INFO;	3✔
230	switch (status_text.type) {	3✔
231	case TelemetryServer::StatusTextType::Emergency:	×
232	type = MAV_SEVERITY_EMERGENCY;	×
233	break;	×
234	case TelemetryServer::StatusTextType::Alert:	×
235	type = MAV_SEVERITY_ALERT;	×
236	break;	×
237	case TelemetryServer::StatusTextType::Critical:	×
238	type = MAV_SEVERITY_CRITICAL;	×
239	break;	×
240	case TelemetryServer::StatusTextType::Error:	×
241	type = MAV_SEVERITY_ERROR;	×
242	break;	×
243	case TelemetryServer::StatusTextType::Warning:	×
244	type = MAV_SEVERITY_WARNING;	×
245	break;	×
246	case TelemetryServer::StatusTextType::Notice:	×
247	type = MAV_SEVERITY_NOTICE;	×
248	break;	×
249	case TelemetryServer::StatusTextType::Info:	3✔
250	type = MAV_SEVERITY_INFO;	3✔
251	break;	3✔
252	case TelemetryServer::StatusTextType::Debug:	×
253	type = MAV_SEVERITY_DEBUG;	×
254	break;	×
255	default:	×
256	LogWarn() << "Unknown StatusText severity";	×
257	type = MAV_SEVERITY_INFO;	×
258	break;	×
259	}
260
261	// Prevent memcpy in mavlink function to read outside of allocated data.
262	status_text.text.resize(sizeof(mavlink_statustext_t::text));	3✔
263
264	mavlink_msg_statustext_pack(	3✔
265	_server_component_impl->get_own_system_id(),	3✔
266	_server_component_impl->get_own_component_id(),	3✔
267	&msg,
268	type,
269	status_text.text.data(),	3✔
270	0,
271	0);
272
273	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	3✔
274	TelemetryServer::Result::Unsupported;	3✔
275	}
276
277	TelemetryServer::Result TelemetryServerImpl::publish_odometry(TelemetryServer::Odometry odometry)	×
278	{
279	UNUSED(odometry);	×
280
281	// TODO :)
282	return {};	×
283	}
284
285	TelemetryServer::Result TelemetryServerImpl::publish_position_velocity_ned(	×
286	TelemetryServer::PositionVelocityNed position_velocity_ned)
287	{
288	mavlink_message_t msg;	×
289	mavlink_msg_local_position_ned_pack(	×
290	_server_component_impl->get_own_system_id(),	×
291	_server_component_impl->get_own_component_id(),	×
292	&msg,
293	get_boot_time_ms(),	×
294	position_velocity_ned.position.north_m,
295	position_velocity_ned.position.east_m,
296	position_velocity_ned.position.down_m,
297	position_velocity_ned.velocity.north_m_s,
298	position_velocity_ned.velocity.east_m_s,
299	position_velocity_ned.velocity.down_m_s);
300
301	add_msg_cache(MAVLINK_MSG_ID_LOCAL_POSITION_NED, msg);	×
302
303	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
304	TelemetryServer::Result::Unsupported;	×
305	}
306
307	TelemetryServer::Result
308	TelemetryServerImpl::publish_ground_truth(TelemetryServer::GroundTruth ground_truth)	×
309	{
310	UNUSED(ground_truth);	×
311
312	// TODO :)
313	return {};	×
314	}
315
316	TelemetryServer::Result TelemetryServerImpl::publish_imu(TelemetryServer::Imu imu)	×
317	{
318	UNUSED(imu);	×
319
320	// TODO :)
321	return {};	×
322	}
323
324	TelemetryServer::Result TelemetryServerImpl::publish_scaled_imu(TelemetryServer::Imu imu)	×
325	{
326	UNUSED(imu);	×
327
328	// TODO :)
329	return {};	×
330	}
331
332	TelemetryServer::Result TelemetryServerImpl::publish_raw_imu(TelemetryServer::Imu imu)	×
333	{
334	UNUSED(imu);	×
335
336	// TODO :)
337	return {};	×
338	}
339
340	TelemetryServer::Result TelemetryServerImpl::publish_unix_epoch_time(uint64_t time_us)	×
341	{
342	UNUSED(time_us);	×
343
344	// TODO :)
345	return {};	×
346	}
347
348	TelemetryServer::Result TelemetryServerImpl::publish_sys_status(	×
349	TelemetryServer::Battery battery,
350	bool rc_receiver_status,
351	bool gyro_status,
352	bool accel_status,
353	bool mag_status,
354	bool gps_status)
355	{
356	int32_t sensors = 0;	×
357
358	if (rc_receiver_status) {	×
359	sensors \|= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;	×
360	}
361	if (gyro_status) {	×
362	sensors \|= MAV_SYS_STATUS_SENSOR_3D_GYRO;	×
363	}
364	if (accel_status) {	×
365	sensors \|= MAV_SYS_STATUS_SENSOR_3D_ACCEL;	×
366	}
367	if (mag_status) {	×
368	sensors \|= MAV_SYS_STATUS_SENSOR_3D_MAG;	×
369	}
370	if (gps_status) {	×
371	sensors \|= MAV_SYS_STATUS_SENSOR_GPS;	×
372	}
373	mavlink_message_t msg;	×
374	mavlink_msg_sys_status_pack(	×
375	_server_component_impl->get_own_system_id(),	×
376	_server_component_impl->get_own_component_id(),	×
377	&msg,
378	sensors,
379	sensors,
380	sensors,
381	0,
382	static_cast<uint16_t>(static_cast<double>(battery.voltage_v) * 1E3),	×
383	-1,
384	static_cast<uint16_t>(static_cast<double>(battery.remaining_percent) * 1E2),	×
385	0,
386	0,
387	0,
388	0,
389	0,
390	0,
391	0,
392	0,
393	0);
394
395	add_msg_cache(MAVLINK_MSG_ID_SYS_STATUS, msg);	×
396
397	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
398	TelemetryServer::Result::Unsupported;	×
399	}
400
401	uint8_t to_mav_vtol_state(TelemetryServer::VtolState vtol_state)	×
402	{
403	switch (vtol_state) {	×
404	case TelemetryServer::VtolState::Undefined:	×
405	return MAV_VTOL_STATE_UNDEFINED;	×
406	case TelemetryServer::VtolState::TransitionToFw:	×
407	return MAV_VTOL_STATE_TRANSITION_TO_FW;	×
408	case TelemetryServer::VtolState::TransitionToMc:	×
409	return MAV_VTOL_STATE_TRANSITION_TO_MC;	×
410	case TelemetryServer::VtolState::Mc:	×
411	return MAV_VTOL_STATE_MC;	×
412	case TelemetryServer::VtolState::Fw:	×
413	return MAV_VTOL_STATE_FW;	×
414	default:	×
415	return MAV_VTOL_STATE_UNDEFINED;	×
416	}
417	}
418
419	uint8_t to_mav_landed_state(TelemetryServer::LandedState landed_state)	×
420	{
421	switch (landed_state) {	×
422	case TelemetryServer::LandedState::InAir:	×
423	return MAV_LANDED_STATE_IN_AIR;	×
424	case TelemetryServer::LandedState::TakingOff:	×
425	return MAV_LANDED_STATE_TAKEOFF;	×
426	case TelemetryServer::LandedState::Landing:	×
427	return MAV_LANDED_STATE_LANDING;	×
428	case TelemetryServer::LandedState::OnGround:	×
429	return MAV_LANDED_STATE_ON_GROUND;	×
430	default:	×
431	return MAV_LANDED_STATE_UNDEFINED;	×
432	}
433	}
434
435	TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(	×
436	TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state)
437	{
438	mavlink_message_t msg;	×
439	mavlink_msg_extended_sys_state_pack(	×
440	_server_component_impl->get_own_system_id(),	×
441	_server_component_impl->get_own_component_id(),	×
442	&msg,
443	to_mav_vtol_state(vtol_state),	×
444	to_mav_landed_state(landed_state));	×
445
446	add_msg_cache(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, msg);	×
447
448	return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success :	×
449	TelemetryServer::Result::Unsupported;	×
450	}
451
452	void TelemetryServerImpl::add_msg_cache(uint64_t id, mavlink_message_t& msg)	×
453	{
454	std::unique_lock<std::mutex> lock(_interval_mutex);	×
455	_msg_cache.insert_or_assign(id, msg);	×
456	}	×
457
458	} // namespace mavsdk

mavlink / MAVSDK / 4053943449

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