11#include < Transmitter.hpp>
22#include < unistd.h>
3- #include < CircularBuffer.hpp >
3+ #include < mavsdk/log_callback.h >
44
55namespace txr
66{
77
8- #if defined(DOCKER_BUILD)
9- static char VERSION_FILE_NAME[] = " /data/version.txt"
10- #else
11- static char VERSION_FILE_NAME[] = " /tmp/rid-transmitter/version.txt"
12- #endif
13-
148Transmitter::Transmitter (const txr::Settings& settings)
159 : _settings(settings)
1610{
17- // If version has changed reset parameters to default
18- std::string version (APP_GIT_VERSION);
19- std::string saved_version (get_sw_version ());
20-
21- if (saved_version != version) {
22- params::set_defaults ();
23- set_sw_version (version);
24- LOG (GREEN_TEXT " New software version! Resetting parameters"
25- }
26-
27- LOG (GREEN_TEXT " Version: " " %s" c_str ());
28-
29- // Load parameters from file system into RAM
30- params::load ();
11+ // Disable mavsdk noise
12+ mavsdk::log::subscribe ([](...) {
13+ // https://mavsdk.mavlink.io/main/en/cpp/guide/logging.html
14+ return true ;
15+ });
3116}
3217
3318bool Transmitter::start ()
@@ -39,53 +24,68 @@ bool Transmitter::start()
3924 return false ;
4025 }
4126
42- // // Setup MAVLink
43- _mavlink = std::make_shared<mavlink::Mavlink>(_settings.mavlink_settings );
27+ LOG (" Waiting for MAVSDK connection: %s" mavsdk_connection_url .c_str ());
4428
45- // Provide PARAM_REQUEST_LIST and PARAM_SET callbacks for our application
46- _mavlink-> enable_parameters (
47- std::bind (&Transmitter::mavlink_param_request_list_cb, this ),
48- std::bind (&Transmitter::mavlink_param_set_cb, this , std::placeholders::_1)
49- );
29+ while (! wait_for_mavsdk_connection ( 3 )) {
30+ if (_should_exit) {
31+ return false ;
32+ }
33+ }
5034
51- _mavlink->subscribe_to_message (MAVLINK_MSG_ID_HEARTBEAT, [this ](const mavlink_message_t & message) {
35+ _mavlink->subscribe_message (MAVLINK_MSG_ID_HEARTBEAT, [this ](const mavlink_message_t & message) {
5236 if (message.sysid == 1 && message.compid == 1 ) {
37+ // LOG("MAVLINK_MSG_ID_HEARTBEAT: %u / %u", message.sysid, message.compid);
5338 std::lock_guard<std::mutex> lock (_heartbeat_mutex);
5439 mavlink_msg_heartbeat_decode (&message, &_heartbeat_msg);
5540 }
5641 });
5742
58- _mavlink->subscribe_to_message (MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, [this ](const mavlink_message_t & message) {
43+ _mavlink->subscribe_message (MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, [this ](const mavlink_message_t & message) {
5944 // LOG("MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: %u / %u", message.sysid, message.compid);
6045 std::lock_guard<std::mutex> lock (_location_mutex);
6146 mavlink_msg_open_drone_id_location_decode (&message, &_location_msg);
6247 });
6348
64- _mavlink->subscribe_to_message (MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, [this ](const mavlink_message_t & message) {
49+ _mavlink->subscribe_message (MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, [this ](const mavlink_message_t & message) {
6550 // LOG("MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: %u / %u", message.sysid, message.compid);
6651 std::lock_guard<std::mutex> lock (_system_mutex);
6752 mavlink_msg_open_drone_id_system_decode (&message, &_system_msg);
6853 });
6954
70- auto result = _mavlink->start ();
71-
72- if (result != mavlink::ConnectionResult::Success) {
73- LOG (RED_TEXT " Mavlink connection start failed"
74- return false ;
75- }
76-
7755 return true ;
7856}
7957
8058void Transmitter::stop ()
8159{
82- if (_mavlink.get ()) _mavlink->stop ();
83-
8460 if (_bluetooth.get ()) _bluetooth->stop ();
8561
8662 _should_exit.store (true );
8763}
8864
65+ bool Transmitter::wait_for_mavsdk_connection (double timeout_s)
66+ {
67+ auto config = mavsdk::Mavsdk::Configuration (mavsdk::ComponentType::RemoteId);
68+ _mavsdk = std::make_shared<mavsdk::Mavsdk>(config);
69+
70+ auto result = _mavsdk->add_any_connection (_settings.mavsdk_connection_url );
71+
72+ if (result != mavsdk::ConnectionResult::Success) {
73+ return false ;
74+ }
75+
76+ auto system = _mavsdk->first_autopilot (timeout_s);
77+
78+ if (!system) {
79+ return false ;
80+ }
81+
82+ LOG (" Connected to autopilot"
83+ _mavlink = std::make_shared<mavsdk::MavlinkPassthrough>(system.value ());
84+
85+ return true ;
86+ }
87+
88+
8989void Transmitter::run_state_machine ()
9090{
9191 uint64_t loop_rate_ms = 200 ;
@@ -103,10 +103,6 @@ void Transmitter::run_state_machine()
103103 _bluetooth->enable_le_extended_advertising ();
104104 }
105105
106- if (params::updated ()) {
107- params::load ();
108- }
109-
110106 // Fill in the data from mavlink messages
111107 struct ODID_UAS_Data data = {};
112108 // Basic ID
@@ -211,68 +207,4 @@ void Transmitter::send_single_messages(struct ODID_UAS_Data* data)
211207 }
212208}
213209
214- std::vector<mavlink::Parameter> Transmitter::mavlink_param_request_list_cb ()
215- {
216- std::vector<mavlink::Parameter> mavlink_parameters;
217- auto params = params::parameters (); // Copy parameters from working set
218- uint16_t count = 0 ;
219-
220- for (auto & [name, value] : params) {
221- mavlink::Parameter p = {
222- .name = name,
223- .float_value = value,
224- .index = count++,
225- .total_count = (uint16_t )params.size (),
226- .type = MAV_PARAM_TYPE_REAL32 // We only use floats
227- };
228-
229- mavlink_parameters.emplace_back (p);
230- }
231-
232- return mavlink_parameters;
233- }
234-
235- bool Transmitter::mavlink_param_set_cb (mavlink::Parameter* param)
236- {
237- bool succes = params::set_parameter (param->name , param->float_value );
238-
239- if (succes) {
240- auto params = params::parameters ();
241- param->index = distance (params.begin (), params.find (param->name ));
242- }
243-
244- return succes;
245- }
246-
247- const std::string Transmitter::get_sw_version ()
248- {
249- std::string version;
250-
251- std::ifstream infile (VERSION_FILE_NAME);
252- std::stringstream is;
253-
254- if (!is.good ()) {
255- return version;
256- }
257-
258- is << infile.rdbuf ();
259- infile.close ();
260-
261- std::string line;
262-
263- if (std::getline (is, line)) {
264- version = line;
265- }
266-
267- return version;
268- }
269-
270- void Transmitter::set_sw_version (const std::string& version)
271- {
272- std::ofstream outfile;
273- outfile.open (VERSION_FILE_NAME, std::ofstream::out | std::ofstream::trunc);
274- outfile << version << std::endl;
275- outfile.close ();
276- }
277-
278- } // end namespace txr
210+ } // end namespace txr
0 commit comments