4578731844

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #2012 from mavlink/rename-parent

Run Details

885 of 885 new or added lines in 31 files covered. (100.0%)

7416 of 24250 relevant lines covered (30.58%)

21.54 hits per line

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

0.0

/src/mavsdk/plugins/mocap/mocap_impl.cpp

#include "mocap_impl.h"
#include "system.h"
#include "px4_custom_mode.h"
#include <array>
#include <ctime>

namespace mavsdk {

void MocapImpl::init() {}

void MocapImpl::deinit() {}

void MocapImpl::enable() {}

void MocapImpl::disable() {}

MocapImpl::MocapImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

MocapImpl::MocapImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

MocapImpl::~MocapImpl()
{
    _system_impl->unregister_plugin(this);
}

Mocap::Result MocapImpl::set_vision_position_estimate(
    const Mocap::VisionPositionEstimate& vision_position_estimate)
{
    if (!_system_impl->is_connected()) {
        return Mocap::Result::NoSystem;
    }

    return send_vision_position_estimate(vision_position_estimate);
}

Mocap::Result
MocapImpl::set_attitude_position_mocap(const Mocap::AttitudePositionMocap& attitude_position_mocap)
{
    if (!_system_impl->is_connected()) {
        return Mocap::Result::NoSystem;
    }

    return send_attitude_position_mocap(attitude_position_mocap);
}

Mocap::Result MocapImpl::set_odometry(const Mocap::Odometry& odometry)
{
    if (!_system_impl->is_connected()) {
        return Mocap::Result::NoSystem;
    }

    return send_odometry(odometry);
}

Mocap::Result MocapImpl::send_vision_position_estimate(
    const Mocap::VisionPositionEstimate& vision_position_estimate)
{
    const uint64_t autopilot_time_usec =
        (!vision_position_estimate.time_usec) ?
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time().now().time_since_epoch())
                .count() :
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time()
                    .time_in(SystemTimePoint(
                        std::chrono::microseconds(vision_position_estimate.time_usec)))
                    .time_since_epoch())
                .count();

    std::array<float, 21> covariance{};

    // The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.

    if (vision_position_estimate.pose_covariance.covariance_matrix.size() == 21) {
        std::copy(
            vision_position_estimate.pose_covariance.covariance_matrix.begin(),
            vision_position_estimate.pose_covariance.covariance_matrix.end(),
            covariance.begin());
    } else if (
        vision_position_estimate.pose_covariance.covariance_matrix.size() == 1 &&
        std::isnan(vision_position_estimate.pose_covariance.covariance_matrix[0])) {
        covariance[0] = NAN;
    } else {
        return Mocap::Result::InvalidRequestData;
    }

    mavlink_message_t message;
    mavlink_msg_vision_position_estimate_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &message,
        autopilot_time_usec,
        vision_position_estimate.position_body.x_m,
        vision_position_estimate.position_body.y_m,
        vision_position_estimate.position_body.z_m,
        vision_position_estimate.angle_body.roll_rad,
        vision_position_estimate.angle_body.pitch_rad,
        vision_position_estimate.angle_body.yaw_rad,
        covariance.data(),
        0); // FIXME: reset_counter not set

    return _system_impl->send_message(message) ? Mocap::Result::Success :
                                                 Mocap::Result::ConnectionError;
}

Mocap::Result
MocapImpl::send_attitude_position_mocap(const Mocap::AttitudePositionMocap& attitude_position_mocap)
{
    const uint64_t autopilot_time_usec =
        (!attitude_position_mocap.time_usec) ?
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time().now().time_since_epoch())
                .count() :
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time()
                    .time_in(SystemTimePoint(
                        std::chrono::microseconds(attitude_position_mocap.time_usec)))
                    .time_since_epoch())
                .count();

    mavlink_message_t message;

    std::array<float, 4> q{};
    q[0] = attitude_position_mocap.q.w;
    q[1] = attitude_position_mocap.q.x;
    q[2] = attitude_position_mocap.q.y;
    q[3] = attitude_position_mocap.q.z;

    std::array<float, 21> covariance{};

    // The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.

    if (attitude_position_mocap.pose_covariance.covariance_matrix.size() == 21) {
        std::copy(
            attitude_position_mocap.pose_covariance.covariance_matrix.begin(),
            attitude_position_mocap.pose_covariance.covariance_matrix.end(),
            covariance.begin());
    } else if (
        attitude_position_mocap.pose_covariance.covariance_matrix.size() == 1 &&
        std::isnan(attitude_position_mocap.pose_covariance.covariance_matrix[0])) {
        covariance[0] = NAN;
    } else {
        return Mocap::Result::InvalidRequestData;
    }

    mavlink_msg_att_pos_mocap_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &message,
        autopilot_time_usec,
        q.data(),
        attitude_position_mocap.position_body.x_m,
        attitude_position_mocap.position_body.y_m,
        attitude_position_mocap.position_body.z_m,
        covariance.data());

    return _system_impl->send_message(message) ? Mocap::Result::Success :
                                                 Mocap::Result::ConnectionError;
}

Mocap::Result MocapImpl::send_odometry(const Mocap::Odometry& odometry)
{
    const uint64_t autopilot_time_usec =
        (!odometry.time_usec) ?
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time().now().time_since_epoch())
                .count() :
            std::chrono::duration_cast<std::chrono::microseconds>(
                _system_impl->get_autopilot_time()
                    .time_in(SystemTimePoint(std::chrono::microseconds(odometry.time_usec)))
                    .time_since_epoch())
                .count();

    mavlink_message_t message;

    std::array<float, 4> q{};
    q[0] = odometry.q.w;
    q[1] = odometry.q.x;
    q[2] = odometry.q.y;
    q[3] = odometry.q.z;

    std::array<float, 21> pose_covariance{};
    std::array<float, 21> velocity_covariance{};

    // The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.

    if (odometry.pose_covariance.covariance_matrix.size() == 21) {
        std::copy(
            odometry.pose_covariance.covariance_matrix.begin(),
            odometry.pose_covariance.covariance_matrix.end(),
            pose_covariance.begin());
    } else if (
        odometry.pose_covariance.covariance_matrix.size() == 1 &&
        std::isnan(odometry.pose_covariance.covariance_matrix[0])) {
        pose_covariance[0] = NAN;
    } else {
        return Mocap::Result::InvalidRequestData;
    }

    if (odometry.velocity_covariance.covariance_matrix.size() == 21) {
        std::copy(
            odometry.velocity_covariance.covariance_matrix.begin(),
            odometry.velocity_covariance.covariance_matrix.end(),
            velocity_covariance.begin());
    } else if (
        odometry.velocity_covariance.covariance_matrix.size() == 1 &&
        std::isnan(odometry.velocity_covariance.covariance_matrix[0])) {
        velocity_covariance[0] = NAN;
    } else {
        return Mocap::Result::InvalidRequestData;
    }

    mavlink_msg_odometry_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &message,
        autopilot_time_usec,
        static_cast<uint8_t>(odometry.frame_id),
        static_cast<uint8_t>(MAV_FRAME_BODY_FRD),
        odometry.position_body.x_m,
        odometry.position_body.y_m,
        odometry.position_body.z_m,
        q.data(),
        odometry.speed_body.x_m_s,
        odometry.speed_body.y_m_s,
        odometry.speed_body.z_m_s,
        odometry.angular_velocity_body.roll_rad_s,
        odometry.angular_velocity_body.pitch_rad_s,
        odometry.angular_velocity_body.yaw_rad_s,
        pose_covariance.data(),
        velocity_covariance.data(),
        0,
        MAV_ESTIMATOR_TYPE_MOCAP,
        0);

    return _system_impl->send_message(message) ? Mocap::Result::Success :
                                                 Mocap::Result::ConnectionError;
}

} // namespace mavsdk

1	#include "mocap_impl.h"
2	#include "system.h"
3	#include "px4_custom_mode.h"
4	#include <array>
5	#include <ctime>
6
7	namespace mavsdk {
8
9	void MocapImpl::init() {}	×
10
11	void MocapImpl::deinit() {}	×
12
13	void MocapImpl::enable() {}	×
14
15	void MocapImpl::disable() {}	×
16
17	MocapImpl::MocapImpl(System& system) : PluginImplBase(system)	×
18	{
19	_system_impl->register_plugin(this);	×
20	}	×
21
22	MocapImpl::MocapImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
23	{
24	_system_impl->register_plugin(this);	×
25	}	×
26
27	MocapImpl::~MocapImpl()	×
28	{
29	_system_impl->unregister_plugin(this);	×
30	}	×
31
32	Mocap::Result MocapImpl::set_vision_position_estimate(	×
33	const Mocap::VisionPositionEstimate& vision_position_estimate)
34	{
35	if (!_system_impl->is_connected()) {	×
36	return Mocap::Result::NoSystem;	×
37	}
38
39	return send_vision_position_estimate(vision_position_estimate);	×
40	}
41
42	Mocap::Result
43	MocapImpl::set_attitude_position_mocap(const Mocap::AttitudePositionMocap& attitude_position_mocap)	×
44	{
45	if (!_system_impl->is_connected()) {	×
46	return Mocap::Result::NoSystem;	×
47	}
48
49	return send_attitude_position_mocap(attitude_position_mocap);	×
50	}
51
52	Mocap::Result MocapImpl::set_odometry(const Mocap::Odometry& odometry)	×
53	{
54	if (!_system_impl->is_connected()) {	×
55	return Mocap::Result::NoSystem;	×
56	}
57
58	return send_odometry(odometry);	×
59	}
60
61	Mocap::Result MocapImpl::send_vision_position_estimate(	×
62	const Mocap::VisionPositionEstimate& vision_position_estimate)
63	{
64	const uint64_t autopilot_time_usec =
65	(!vision_position_estimate.time_usec) ?	×
66	std::chrono::duration_cast<std::chrono::microseconds>(	×
67	_system_impl->get_autopilot_time().now().time_since_epoch())	×
68	.count() :	×
69	std::chrono::duration_cast<std::chrono::microseconds>(	×
70	_system_impl->get_autopilot_time()	×
71	.time_in(SystemTimePoint(	×
72	std::chrono::microseconds(vision_position_estimate.time_usec)))	×
73	.time_since_epoch())	×
74	.count();	×
75
76	std::array<float, 21> covariance{};	×
77
78	// The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.
79
80	if (vision_position_estimate.pose_covariance.covariance_matrix.size() == 21) {	×
81	std::copy(	×
82	vision_position_estimate.pose_covariance.covariance_matrix.begin(),
83	vision_position_estimate.pose_covariance.covariance_matrix.end(),
84	covariance.begin());
85	} else if (	×
86	vision_position_estimate.pose_covariance.covariance_matrix.size() == 1 &&	×
87	std::isnan(vision_position_estimate.pose_covariance.covariance_matrix[0])) {	×
88	covariance[0] = NAN;	×
89	} else {
90	return Mocap::Result::InvalidRequestData;	×
91	}
92
93	mavlink_message_t message;	×
94	mavlink_msg_vision_position_estimate_pack(	×
95	_system_impl->get_own_system_id(),	×
96	_system_impl->get_own_component_id(),	×
97	&message,
98	autopilot_time_usec,
99	vision_position_estimate.position_body.x_m,	×
100	vision_position_estimate.position_body.y_m,	×
101	vision_position_estimate.position_body.z_m,	×
102	vision_position_estimate.angle_body.roll_rad,	×
103	vision_position_estimate.angle_body.pitch_rad,	×
104	vision_position_estimate.angle_body.yaw_rad,	×
105	covariance.data(),	×
106	0); // FIXME: reset_counter not set
107
108	return _system_impl->send_message(message) ? Mocap::Result::Success :	×
109	Mocap::Result::ConnectionError;	×
110	}
111
112	Mocap::Result
113	MocapImpl::send_attitude_position_mocap(const Mocap::AttitudePositionMocap& attitude_position_mocap)	×
114	{
115	const uint64_t autopilot_time_usec =
116	(!attitude_position_mocap.time_usec) ?	×
117	std::chrono::duration_cast<std::chrono::microseconds>(	×
118	_system_impl->get_autopilot_time().now().time_since_epoch())	×
119	.count() :	×
120	std::chrono::duration_cast<std::chrono::microseconds>(	×
121	_system_impl->get_autopilot_time()	×
122	.time_in(SystemTimePoint(	×
123	std::chrono::microseconds(attitude_position_mocap.time_usec)))	×
124	.time_since_epoch())	×
125	.count();	×
126
127	mavlink_message_t message;	×
128
129	std::array<float, 4> q{};	×
130	q[0] = attitude_position_mocap.q.w;	×
131	q[1] = attitude_position_mocap.q.x;	×
132	q[2] = attitude_position_mocap.q.y;	×
133	q[3] = attitude_position_mocap.q.z;	×
134
135	std::array<float, 21> covariance{};	×
136
137	// The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.
138
139	if (attitude_position_mocap.pose_covariance.covariance_matrix.size() == 21) {	×
140	std::copy(	×
141	attitude_position_mocap.pose_covariance.covariance_matrix.begin(),
142	attitude_position_mocap.pose_covariance.covariance_matrix.end(),
143	covariance.begin());
144	} else if (	×
145	attitude_position_mocap.pose_covariance.covariance_matrix.size() == 1 &&	×
146	std::isnan(attitude_position_mocap.pose_covariance.covariance_matrix[0])) {	×
147	covariance[0] = NAN;	×
148	} else {
149	return Mocap::Result::InvalidRequestData;	×
150	}
151
152	mavlink_msg_att_pos_mocap_pack(	×
153	_system_impl->get_own_system_id(),	×
154	_system_impl->get_own_component_id(),	×
155	&message,
156	autopilot_time_usec,
157	q.data(),	×
158	attitude_position_mocap.position_body.x_m,	×
159	attitude_position_mocap.position_body.y_m,	×
160	attitude_position_mocap.position_body.z_m,	×
161	covariance.data());	×
162
163	return _system_impl->send_message(message) ? Mocap::Result::Success :	×
164	Mocap::Result::ConnectionError;	×
165	}
166
167	Mocap::Result MocapImpl::send_odometry(const Mocap::Odometry& odometry)	×
168	{
169	const uint64_t autopilot_time_usec =
170	(!odometry.time_usec) ?	×
171	std::chrono::duration_cast<std::chrono::microseconds>(	×
172	_system_impl->get_autopilot_time().now().time_since_epoch())	×
173	.count() :	×
174	std::chrono::duration_cast<std::chrono::microseconds>(	×
175	_system_impl->get_autopilot_time()	×
176	.time_in(SystemTimePoint(std::chrono::microseconds(odometry.time_usec)))	×
177	.time_since_epoch())	×
178	.count();	×
179
180	mavlink_message_t message;	×
181
182	std::array<float, 4> q{};	×
183	q[0] = odometry.q.w;	×
184	q[1] = odometry.q.x;	×
185	q[2] = odometry.q.y;	×
186	q[3] = odometry.q.z;	×
187
188	std::array<float, 21> pose_covariance{};	×
189	std::array<float, 21> velocity_covariance{};	×
190
191	// The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.
192
193	if (odometry.pose_covariance.covariance_matrix.size() == 21) {	×
194	std::copy(	×
195	odometry.pose_covariance.covariance_matrix.begin(),
196	odometry.pose_covariance.covariance_matrix.end(),
197	pose_covariance.begin());
198	} else if (	×
199	odometry.pose_covariance.covariance_matrix.size() == 1 &&	×
200	std::isnan(odometry.pose_covariance.covariance_matrix[0])) {	×
201	pose_covariance[0] = NAN;	×
202	} else {
203	return Mocap::Result::InvalidRequestData;	×
204	}
205
206	if (odometry.velocity_covariance.covariance_matrix.size() == 21) {	×
207	std::copy(	×
208	odometry.velocity_covariance.covariance_matrix.begin(),
209	odometry.velocity_covariance.covariance_matrix.end(),
210	velocity_covariance.begin());
211	} else if (	×
212	odometry.velocity_covariance.covariance_matrix.size() == 1 &&	×
213	std::isnan(odometry.velocity_covariance.covariance_matrix[0])) {	×
214	velocity_covariance[0] = NAN;	×
215	} else {
216	return Mocap::Result::InvalidRequestData;	×
217	}
218
219	mavlink_msg_odometry_pack(	×
220	_system_impl->get_own_system_id(),	×
221	_system_impl->get_own_component_id(),	×
222	&message,
223	autopilot_time_usec,
224	static_cast<uint8_t>(odometry.frame_id),	×
225	static_cast<uint8_t>(MAV_FRAME_BODY_FRD),
226	odometry.position_body.x_m,	×
227	odometry.position_body.y_m,	×
228	odometry.position_body.z_m,	×
229	q.data(),	×
230	odometry.speed_body.x_m_s,	×
231	odometry.speed_body.y_m_s,	×
232	odometry.speed_body.z_m_s,	×
233	odometry.angular_velocity_body.roll_rad_s,	×
234	odometry.angular_velocity_body.pitch_rad_s,	×
235	odometry.angular_velocity_body.yaw_rad_s,	×
236	pose_covariance.data(),	×
237	velocity_covariance.data(),	×
238	0,
239	MAV_ESTIMATOR_TYPE_MOCAP,
240	0);
241
242	return _system_impl->send_message(message) ? Mocap::Result::Success :	×
243	Mocap::Result::ConnectionError;	×
244	}
245
246	} // namespace mavsdk

mavlink / MAVSDK / 4578731844

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