-
Notifications
You must be signed in to change notification settings - Fork 983
/
gps_rtk.cpp
159 lines (139 loc) · 5.2 KB
/
gps_rtk.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
/*
* Copyright 2018 Alexis Paques.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
/**
* @brief GPS RTK plugin
* @file gps_rtk.cpp
* @author Alexis Paques <[email protected]>
*
* @addtogroup plugin
* @{
*/
#include <algorithm>
#include "rcpputils/asserts.hpp"
#include "mavros/mavros_uas.hpp"
#include "mavros/plugin.hpp"
#include "mavros/plugin_filter.hpp"
#include "mavros_msgs/msg/rtcm.hpp"
#include "mavros_msgs/msg/rtk_baseline.hpp"
namespace mavros
{
namespace extra_plugins
{
using namespace std::placeholders; // NOLINT
/**
* @brief GPS RTK plugin
* @plugin gps_rtk
*
* 1. Publish the RTCM messages from ROS to the FCU
* 2. Publish RTK baseline data from the FCU to ROS
*/
class GpsRtkPlugin : public plugin::Plugin
{
public:
explicit GpsRtkPlugin(plugin::UASPtr uas_)
: Plugin(uas_, "gps_rtk")
{
gps_rtk_sub =
node->create_subscription<mavros_msgs::msg::RTCM>(
"~/send_rtcm", 10,
std::bind(&GpsRtkPlugin::rtcm_cb, this, _1));
// TODO(vooon): set QoS for latched topic
rtk_baseline_pub = node->create_publisher<mavros_msgs::msg::RTKBaseline>("~/rtk_baseline", 1);
}
Subscriptions get_subscriptions() override
{
return {
make_handler(&GpsRtkPlugin::handle_baseline_msg)
};
}
private:
rclcpp::Subscription<mavros_msgs::msg::RTCM>::SharedPtr gps_rtk_sub;
rclcpp::Publisher<mavros_msgs::msg::RTKBaseline>::SharedPtr rtk_baseline_pub;
mavros_msgs::msg::RTKBaseline rtk_baseline_;
std::atomic_uint rtcm_seq;
/* -*- callbacks -*- */
/**
* @brief Handle mavros_msgs::RTCM message
* It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection.
* Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA
* @param msg Received ROS msg
*/
void rtcm_cb(const mavros_msgs::msg::RTCM::SharedPtr msg)
{
mavlink::common::msg::GPS_RTCM_DATA rtcm_data = {};
const size_t max_frag_len = rtcm_data.data.size();
uint8_t seq_u5 = uint8_t(rtcm_seq.fetch_add(1) & 0x1F) << 3;
if (msg->data.size() > 4 * max_frag_len) {
RCLCPP_ERROR(
get_logger(),
"gps_rtk: RTCM message received is bigger than the maximal possible size.");
return;
}
auto data_it = msg->data.begin();
auto end_it = msg->data.end();
if (msg->data.size() <= max_frag_len) {
rtcm_data.len = msg->data.size();
rtcm_data.flags = seq_u5;
std::copy(data_it, end_it, rtcm_data.data.begin());
std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0);
uas->send_message(rtcm_data);
} else {
for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) {
uint8_t len = std::min(static_cast<size_t>(std::distance(data_it, end_it)), max_frag_len);
rtcm_data.flags = 1; // LSB set indicates message is fragmented
rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id
rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id
rtcm_data.len = len;
std::copy(data_it, data_it + len, rtcm_data.data.begin());
std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0);
uas->send_message(rtcm_data);
std::advance(data_it, len);
}
}
}
/* MAvlink msg handlers */
/**
* @brief Publish GPS_RTK message (MAvlink Common) received from FCU.
* The message is already decoded by Mavlink, we only need to convert to ROS.
* Details and units: https://mavlink.io/en/messages/common.html#GPS_RTK
*/
void handle_baseline_msg(
const mavlink::mavlink_message_t * msg [[maybe_unused]],
mavlink::common::msg::GPS_RTK & rtk_bsln,
plugin::filter::SystemAndOk filter [[maybe_unused]])
{
// [[[cog:
// import pymavlink.dialects.v20.common as common
//
// for field in common.MAVLink_gps_rtk_message.fieldnames:
// if field in ['time_usec']:
// continue
// cog.outl(f"rtk_baseline_.{field} = rtk_bsln.{field};")
// ]]]
rtk_baseline_.time_last_baseline_ms = rtk_bsln.time_last_baseline_ms;
rtk_baseline_.rtk_receiver_id = rtk_bsln.rtk_receiver_id;
rtk_baseline_.wn = rtk_bsln.wn;
rtk_baseline_.tow = rtk_bsln.tow;
rtk_baseline_.rtk_health = rtk_bsln.rtk_health;
rtk_baseline_.rtk_rate = rtk_bsln.rtk_rate;
rtk_baseline_.nsats = rtk_bsln.nsats;
rtk_baseline_.baseline_coords_type = rtk_bsln.baseline_coords_type;
rtk_baseline_.baseline_a_mm = rtk_bsln.baseline_a_mm;
rtk_baseline_.baseline_b_mm = rtk_bsln.baseline_b_mm;
rtk_baseline_.baseline_c_mm = rtk_bsln.baseline_c_mm;
rtk_baseline_.accuracy = rtk_bsln.accuracy;
rtk_baseline_.iar_num_hypotheses = rtk_bsln.iar_num_hypotheses;
// [[[end]]] (checksum: c123d29c2e0bce3becce956a29ed6152)
rtk_baseline_.header = uas->synchronized_header("", rtk_bsln.time_last_baseline_ms * 1000);
rtk_baseline_pub->publish(rtk_baseline_);
}
};
} // namespace extra_plugins
} // namespace mavros
#include <mavros/mavros_plugin_register_macro.hpp> // NOLINT
MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GpsRtkPlugin)