6568658631

Committed 19 Oct 2023 01:31AM UTC coverage: 31.23% (+0.02%) from 31.215%

Build # 6568658631

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2155 from mavlink/pr-static-fixes

Threading fixes, MAVLink sequence number cleanup

Run Details

1386 of 1386 new or added lines in 46 files covered. (100.0%)

7906 of 25315 relevant lines covered (31.23%)

23.54 hits per line

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

12.11

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

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

namespace mavsdk {

// TODO: fix message cache

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)
{
    // add_msg_cache(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, msg);

    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)
{
    const float q[4] = {};

    // add_msg_cache(MAVLINK_MSG_ID_HOME_POSITION, msg);

    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::Success :
               TelemetryServer::Result::Unsupported;
}

TelemetryServer::Result TelemetryServerImpl::publish_raw_gps(
    TelemetryServer::RawGps raw_gps, TelemetryServer::GpsInfo gps_info)
{
    // add_msg_cache(MAVLINK_MSG_ID_GPS_RAW_INT, msg);

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

    // add_msg_cache(MAVLINK_MSG_ID_BATTERY_STATUS, msg);

    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

    // add_msg_cache(MAVLINK_MSG_ID_DISTANCE_SENSOR, msg);

    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)
{
    // add_msg_cache(MAVLINK_MSG_ID_LOCAL_POSITION_NED, msg);

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

    // add_msg_cache(MAVLINK_MSG_ID_SYS_STATUS, msg);

    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)
{
    // add_msg_cache(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, msg);

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

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

mavlink / MAVSDK / 6568658631

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