Support MAVLink usage in simulation

gazebo/CMakeLists.txt

@@ -15,3 +15,7 @@ set(CMAKE_BUILD_TYPE RelWithDebInfo)
 add_library(flix SHARED simulator.cpp)
 target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
 target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+# Include dir for MAVLink-Arduino library
+target_include_directories(flix PUBLIC $ENV{HOME}/Arduino/libraries/MAVLink)
+target_include_directories(flix PUBLIC $ENV{HOME}/Documents/Arduino/libraries/MAVLink)

gazebo/SBUS.h

@@ -10,7 +10,6 @@ public:
 	SBUS(HardwareSerial& bus) {};
 	void begin() {};
 	bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { // NOTE: on the hardware channels is uint16_t
-		joystickGet();
-		return true;
+		return joystickGet();
 	};
 };

gazebo/flix.h

@@ -8,6 +8,7 @@
 #include "vector.h"
 #include "quaternion.h"
 #include "Arduino.h"
+#include "wifi.h"
 
 #define RC_CHANNELS 6
 
@@ -16,6 +17,8 @@
 #define MOTOR_FRONT_RIGHT 2
 #define MOTOR_REAR_RIGHT 1
 
+#define WIFI_ENABLED 1
+
 float t = NAN;
 float dt;
 float loopFreq;
@@ -40,6 +43,11 @@ void showTable();
 bool motorsActive();
 void cliTestMotor(uint8_t n);
 void printRCCal();
+void processMavlink();
+void sendMavlink();
+void sendMessage(const void *msg);
+void receiveMavlink();
+void handleMavlink(const void *_msg);
 
 // mocks
 void setLED(bool on) {};

gazebo/joystick.h

@@ -41,10 +41,10 @@ void joystickInit() {
 	memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
 }
 
-void joystickGet() {
+bool joystickGet() {
 	if (!joystickInitialized) {
 		joystickInit();
-		return;
+		return false;
 	}
 
 	SDL_JoystickUpdate();
@@ -54,4 +54,5 @@ void joystickGet() {
 	}
 	channels[RC_CHANNEL_MODE] = SDL_JoystickGetButton(joystick, 0) ? 1 : 0;
 	controls[RC_CHANNEL_MODE] = channels[RC_CHANNEL_MODE];
+	return true;
 }

gazebo/simulator.cpp

@@ -25,6 +25,7 @@
 #include "control.ino"
 #include "log.ino"
 #include "cli.ino"
+#include "mavlink.ino"
 #include "lpf.h"
 
 using ignition::math::Vector3d;
@@ -78,6 +79,7 @@ public:
 
 		control();
 		parseInput();
+		processMavlink();
 
 		applyMotorForces();
 		publishTopics();

gazebo/wifi.h

@@ -0,0 +1,44 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// sendWiFi and receiveWiFi implementations for the simulation
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <sys/poll.h>
+#include <gazebo/gazebo.hh>
+
+#define WIFI_UDP_PORT_LOCAL 14580
+#define WIFI_UDP_PORT_REMOTE 14550
+
+int wifiSocket;
+
+void setupWiFi() {
+	wifiSocket = socket(AF_INET, SOCK_DGRAM, 0);
+	sockaddr_in addr;
+	addr.sin_family = AF_INET;
+	addr.sin_addr.s_addr = INADDR_ANY;
+	addr.sin_port = htons(WIFI_UDP_PORT_LOCAL);
+	bind(wifiSocket, (sockaddr *)&addr, sizeof(addr));
+	int broadcast = 1;
+	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
+	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << std::endl;
+}
+
+void sendWiFi(const uint8_t *buf, int len) {
+	if (wifiSocket == 0) setupWiFi();
+	sockaddr_in addr;
+	addr.sin_family = AF_INET;
+	addr.sin_addr.s_addr = INADDR_BROADCAST; // send UDP broadcast
+	addr.sin_port = htons(WIFI_UDP_PORT_REMOTE);
+	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
+}
+
+int receiveWiFi(uint8_t *buf, int len) {
+	struct pollfd pfd = { .fd = wifiSocket, .events = POLLIN };
+	if (poll(&pfd, 1, 0) <= 0) return 0; // check if there is data to read
+	return recv(wifiSocket, buf, len, 0);
+}