Skip to content

Commit b3300c6

Browse files
sbaidachnisbaidachni
authored
Merge pull request #2 from sbaidachni/sbaidachni/mavsdk_test
c and passthrough tests
2 parents 14fe141 + bd6f61b commit b3300c6

File tree

4 files changed

+260
-0
lines changed

4 files changed

+260
-0
lines changed

‎mavsdk_test/Makefile‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
CXX := g++
2+
CXXFLAGS := -Wall -Wextra -pthread -std=c++17
3+
TARGET := mavsdk_test
4+
SRCS := mavsdk_test.cpp
5+
OBJS := $(SRCS:.cpp=.o)
6+
7+
# MAVSDK flags
8+
MAVSDK_CFLAGS := $(shell pkg-config --cflags mavsdk 2>/dev/null)
9+
MAVSDK_LIBS := $(shell pkg-config --libs mavsdk 2>/dev/null)
10+
11+
ifeq ($(strip $(MAVSDK_LIBS)),)
12+
$(warning MAVSDK not found via pkg-config; trying manual flags)
13+
MAVSDK_CFLAGS := -I/usr/local/include -I/usr/include -I/usr/include/mavsdk
14+
MAVSDK_LIBS := -L/usr/lib -lmavsdk
15+
else
16+
# Add mavsdk directory to include path so plugin headers can find plugin_base.h
17+
MAVSDK_CFLAGS += -I/usr/include/mavsdk
18+
endif
19+
20+
CXXFLAGS += $(MAVSDK_CFLAGS)
21+
LDFLAGS += $(MAVSDK_LIBS)
22+
23+
all: $(TARGET)
24+
25+
$(TARGET): $(OBJS)
26+
$(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS)
27+
28+
%.o: %.cpp
29+
$(CXX) $(CXXFLAGS) -c $< -o $@
30+
31+
clean:
32+
rm -f $(TARGET) $(OBJS)
33+
34+
.PHONY: all clean

‎mavsdk_test/mavsdk_test.cpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// introducing MAVSDK
2+
#include <mavsdk/mavsdk.h>
3+
#include <mavsdk/plugins/telemetry/telemetry.h>
4+
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
5+
#include <unistd.h>
6+
#include <stdio.h>
7+
#include <set>
8+
#include <string>
9+
10+
11+
using namespace mavsdk;
12+
13+
int main()
14+
{
15+
mavsdk::Mavsdk mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::ComponentType::GroundStation}};
16+
17+
mavsdk::ConnectionResult connection_result = mavsdk.add_any_connection("serial:///dev/ttyAMA0:115200");
18+
19+
if (connection_result != mavsdk::ConnectionResult::Success) {
20+
printf("Failed to connect to MAVLINK\n");
21+
return 1;
22+
}
23+
printf("Connection successful!\n");
24+
25+
while (mavsdk.systems().size()==0)
26+
{
27+
printf("No MAVSDK system available\n");
28+
sleep(1);
29+
}
30+
31+
auto system = mavsdk.systems().at(0);
32+
printf("System found!\n");
33+
34+
if (!system->is_connected()) {
35+
printf("MAVLINK system not connected\n");
36+
return 1;
37+
}
38+
printf("System connected!\n");
39+
40+
// Use MAVLink Passthrough instead of MavlinkDirect for lower-level access
41+
auto passthrough = mavsdk::MavlinkPassthrough{system};
42+
printf("MavlinkPassthrough created\n");
43+
44+
// Subscribe to incoming messages using passthrough
45+
printf("Subscribing to messages via passthrough...\n");
46+
int message_count = 0;
47+
int heartbeat_count = 0;
48+
int rc_channels_count = 0;
49+
50+
passthrough.subscribe_message(
51+
MAVLINK_MSG_ID_HEARTBEAT,
52+
[&heartbeat_count](const mavlink_message_t& message) {
53+
heartbeat_count++;
54+
mavlink_heartbeat_t heartbeat;
55+
mavlink_msg_heartbeat_decode(&message, &heartbeat);
56+
printf("HEARTBEAT #%d: type=%d, autopilot=%d, base_mode=%d, system_status=%d\n",
57+
heartbeat_count, heartbeat.type, heartbeat.autopilot,
58+
heartbeat.base_mode, heartbeat.system_status);
59+
}
60+
);
61+
62+
passthrough.subscribe_message(
63+
MAVLINK_MSG_ID_RC_CHANNELS,
64+
[&rc_channels_count](const mavlink_message_t& message) {
65+
rc_channels_count++;
66+
mavlink_rc_channels_t rc;
67+
mavlink_msg_rc_channels_decode(&message, &rc);
68+
printf("RC_CHANNELS #%d: chan1=%d, chan2=%d, chan3=%d, chan4=%d, chan5=%d, chan9=%d\n",
69+
rc_channels_count, rc.chan1_raw, rc.chan2_raw, rc.chan3_raw,
70+
rc.chan4_raw, rc.chan5_raw, rc.chan9_raw);
71+
}
72+
);
73+
74+
int i=0;
75+
while (i++<60)
76+
{
77+
if (i % 10 == 0) {
78+
printf("Waiting... %d/60 (total: %d, heartbeats: %d, RC: %d)\n",
79+
i, message_count, heartbeat_count, rc_channels_count);
80+
}
81+
sleep(1);
82+
}
83+
84+
printf("\nFinal count - Total: %d, Heartbeats: %d, RC_CHANNELS: %d\n",
85+
message_count, heartbeat_count, rc_channels_count);
86+
87+
return 0;
88+
}

‎mavsdk_test_c/Makefile‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
CXX := g++
2+
CXXFLAGS := -Wall -Wextra -pthread -std=c++17
3+
TARGET := mavsdk_test
4+
SRCS := mavsdk_test.cpp
5+
OBJS := $(SRCS:.cpp=.o)
6+
7+
# MAVSDK flags
8+
MAVSDK_CFLAGS := $(shell pkg-config --cflags mavsdk 2>/dev/null)
9+
MAVSDK_LIBS := $(shell pkg-config --libs mavsdk 2>/dev/null)
10+
11+
ifeq ($(strip $(MAVSDK_LIBS)),)
12+
$(warning MAVSDK not found via pkg-config; trying manual flags)
13+
MAVSDK_CFLAGS := -I/usr/local/include -I/usr/include -I/usr/include/mavsdk
14+
MAVSDK_LIBS := -L/usr/lib -lmavsdk
15+
else
16+
# Add mavsdk directory to include path so plugin headers can find plugin_base.h
17+
MAVSDK_CFLAGS += -I/usr/include/mavsdk
18+
endif
19+
20+
CXXFLAGS += $(MAVSDK_CFLAGS)
21+
LDFLAGS += $(MAVSDK_LIBS)
22+
23+
all: $(TARGET)
24+
25+
$(TARGET): $(OBJS)
26+
$(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS)
27+
28+
%.o: %.cpp
29+
$(CXX) $(CXXFLAGS) -c $< -o $@
30+
31+
clean:
32+
rm -f $(TARGET) $(OBJS)
33+
34+
.PHONY: all clean

‎mavsdk_test_c/mavsdk_test.cpp‎

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
#include <stdio.h>
2+
#include <stdlib.h>
3+
#include <string.h>
4+
#include <unistd.h>
5+
#include <fcntl.h>
6+
#include <termios.h>
7+
#include <errno.h>
8+
#include <sys/ioctl.h>
9+
10+
#include <mavlink/common/mavlink.h>
11+
12+
int open_serial_port(const char* device, int baud_rate) {
13+
int fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
14+
if (fd == -1) {
15+
perror("open_serial_port: Unable to open");
16+
return -1;
17+
}
18+
19+
// Clear flags
20+
fcntl(fd, F_SETFL, 0);
21+
22+
struct termios options;
23+
tcgetattr(fd, &options);
24+
25+
// Set baud rate
26+
speed_t speed;
27+
switch(baud_rate) {
28+
case 57600: speed = B57600; break;
29+
case 115200: speed = B115200; break;
30+
case 230400: speed = B230400; break;
31+
case 460800: speed = B460800; break;
32+
case 500000: speed = B500000; break;
33+
case 921600: speed = B921600; break;
34+
default: speed = B115200;
35+
printf("Warning: Baudrate %d not explicitly handled, defaulting to 115200\n", baud_rate);
36+
break;
37+
}
38+
cfsetispeed(&options, speed);
39+
cfsetospeed(&options, speed);
40+
41+
// 8N1
42+
options.c_cflag |= (CLOCAL | CREAD);
43+
options.c_cflag &= ~CSIZE;
44+
options.c_cflag |= CS8;
45+
options.c_cflag &= ~PARENB;
46+
options.c_cflag &= ~CSTOPB;
47+
options.c_cflag &= ~CRTSCTS;
48+
49+
// Raw input
50+
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
51+
options.c_iflag &= ~(IXON | IXOFF | IXANY);
52+
options.c_oflag &= ~OPOST;
53+
54+
// Set timeouts
55+
options.c_cc[VMIN] = 0; // Non-blocking read
56+
options.c_cc[VTIME] = 0;
57+
58+
tcsetattr(fd, TCSANOW, &options);
59+
return fd;
60+
}
61+
62+
int main() {
63+
const char* uart_name = "/dev/ttyAMA0";
64+
int baudrate = 115200;
65+
66+
printf("Connecting to %s at %d baud (Raw MAVLink)...\n", uart_name, baudrate);
67+
int fd = open_serial_port(uart_name, baudrate);
68+
if (fd < 0) return 1;
69+
70+
printf("Connected! Waiting for MAVLink messages...\n");
71+
72+
mavlink_status_t status;
73+
mavlink_message_t msg;
74+
uint8_t buf[1024];
75+
int count = 0;
76+
77+
while (1) {
78+
int n = read(fd, buf, sizeof(buf));
79+
if (n > 0) {
80+
for (int i = 0; i < n; ++i) {
81+
// Parse one byte at a time
82+
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
83+
84+
if (msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS) {
85+
mavlink_rc_channels_t rc;
86+
mavlink_msg_rc_channels_decode(&msg, &rc);
87+
printf("RC_CHANNELS: chan9=%d (raw)\n", rc.chan9_raw);
88+
}
89+
}
90+
}
91+
} else if (n < 0) {
92+
if (errno != EAGAIN) {
93+
perror("read error");
94+
usleep(100000); // Wait a bit on error
95+
}
96+
}
97+
98+
// Small sleep to prevent 100% CPU usage in this loop
99+
usleep(1000);
100+
}
101+
102+
close(fd);
103+
return 0;
104+
}

0 commit comments

Comments
 (0)