forked from julianoes/rtk-sender-example
-
Notifications
You must be signed in to change notification settings - Fork 1
/
rtk-sender-example.cpp
76 lines (58 loc) · 2.3 KB
/
rtk-sender-example.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
#include <cstdio>
#include <memory>
#include "PX4-GPSDrivers/src/gps_helper.h"
#include "PX4-GPSDrivers/src/ubx.h"
#include "serial-comms.h"
#include "driver-interface.h"
int main(int argc, char* argv[])
{
if (argc != 4) {
printf("\n");
printf("usage: %s <serial device> <baudrate> <mavlink connection>\n", argv[0]);
printf("e.g.: %s /dev/ttyUSB0 38400 udp://:14550\n", argv[0]);
printf("Note: use baudrate 0 to determine baudrate automatically\n");
return 1;
}
unsigned baudrate = std::stoi(argv[2]);
SerialComms serial_comms;
if (!serial_comms.init(argv[1])) {
return 2;
}
mavsdk::Mavsdk mavsdk;
// Required when connecting to a flight controller directly via USB.
mavsdk::Mavsdk::Configuration config{mavsdk::Mavsdk::Configuration::UsageType::GroundStation};
config.set_always_send_heartbeats(true);
mavsdk.set_configuration(config);
auto connection_result = mavsdk.add_any_connection(argv[3]);
if (connection_result != mavsdk::ConnectionResult::Success) {
printf("Mavsdk connection failed\n");
return 3;
}
DriverInterface driver_interface(serial_comms, mavsdk);
auto driver = std::make_unique<GPSDriverUBX>(
GPSDriverUBX::Interface::UART,
&DriverInterface::callback_entry, &driver_interface,
&driver_interface.gps_pos, &driver_interface.sat_info);
constexpr auto survey_minimum_m = 5;
constexpr auto survey_duration_s = 20;
driver->setSurveyInSpecs(survey_minimum_m * 10000, survey_duration_s);
GPSHelper::GPSConfig gps_config {};
// to test if RTCM is not available
//gps_config.output_mode = GPSHelper::OutputMode::GPS;
gps_config.output_mode = GPSHelper::OutputMode::RTCM;
gps_config.gnss_systems = GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS;
if (driver->configure(baudrate, gps_config) != 0) {
printf("configure failed\n");
return 4;
}
printf("configure done!\n");
while (true) {
// Keep running, and don't stop on timeout.
// Every now and then it timeouts but I'm not sure if that's actually
// warranted given correct messages are still arriving.
const unsigned timeout_ms = 5000;
driver->receive(timeout_ms);
}
printf("timed out\n");
return 0;
}