Support MAVLink usage in simulation

.github/workflows/build.yml

@@ -39,6 +39,8 @@ jobs:
   build_simulator:
     runs-on: ubuntu-latest
     steps:
+    - name: Install Arduino CLI
+      uses: arduino/setup-arduino-cli@v1.1.1
     - uses: actions/checkout@v3
     - name: Install Gazebo
       run: curl -sSL http://get.gazebosim.org | sh
@@ -55,6 +57,8 @@ jobs:
   build_simulator_macos:
     runs-on: macos-latest
     steps:
+    - name: Install Arduino CLI
+      run: brew install arduino-cli
     - uses: actions/checkout@v3
     - name: Clean up python binaries # Workaround for https://github.com/actions/setup-python/issues/577
       run: |

Makefile

@@ -24,7 +24,7 @@ gazebo/build cmake: gazebo/CMakeLists.txt
 	mkdir -p gazebo/build
 	cd gazebo/build && cmake ..
 
-build_simulator: gazebo/build
+build_simulator: .dependencies gazebo/build
 	make -C gazebo/build
 
 simulator: build_simulator

docs/build.md

@@ -9,11 +9,15 @@ cd flix
 
 ## Simulation
 
-Dependencies are [Gazebo Classic simulator](https://classic.gazebosim.org) and [SDL2](https://www.libsdl.org) library.
-
 ### Ubuntu
 
-1. Install Gazebo 11:
+1. Install Arduino CLI:
+
+   ```bash
+   curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/local/bin sh
+   ```
+
+2. Install Gazebo 11:
 
    ```bash
    curl -sSL http://get.gazebosim.org | sh
@@ -26,13 +30,13 @@ Dependencies are [Gazebo Classic simulator](https://classic.gazebosim.org) and [
    source ~/.bashrc
    ```
 
-2. Install SDL2:
+3. Install SDL2 and other dependencies:
 
    ```bash
    sudo apt-get update && sudo apt-get install build-essential libsdl2-dev
    ```
 
-3. Run the simulation:
+4. Run the simulation:
 
    ```bash
    make simulator
@@ -46,10 +50,11 @@ Dependencies are [Gazebo Classic simulator](https://classic.gazebosim.org) and [
    /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
    ```
 
-2. Install Gazebo 11 and SDL2:
+2. Install Arduino CLI, Gazebo 11 and SDL2:
 
    ```bash
    brew tap osrf/simulation
+   brew install arduino-cli
    brew install gazebo11
    brew install sdl2
    ```

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 joystickGet();
-		return true;
 	};
 };

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);
+}