Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Support MAVLink usage in simulation

Oleg Kalachevf718af7f0ed8
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 9f19958..704b6f1 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -39,6 +39,8 @@ jobs:
build_simulator:
runs-on: ubuntu-latest
steps:
+ - name: Install Arduino CLI
+ uses: arduino/[email protected]
- 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: |
diff --git a/Makefile b/Makefile
index c6e2584..58e5f7d 100644
--- a/Makefile
+++ b/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
diff --git a/docs/build.md b/docs/build.md
index af697a4..3984451 100644
--- a/docs/build.md
+++ b/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
```
diff --git a/gazebo/CMakeLists.txt b/gazebo/CMakeLists.txt
index fe1c55f..5bb0eb3 100644
--- a/gazebo/CMakeLists.txt
+++ b/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)
diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index b522574..307a8ba 100644
--- a/gazebo/SBUS.h
+++ b/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();
};
};
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 38540d3..a8ce214 100644
--- a/gazebo/flix.h
+++ b/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) {};
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 2e834a7..04db142 100644
--- a/gazebo/joystick.h
+++ b/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;
}
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index b1ffd46..f681513 100644
--- a/gazebo/simulator.cpp
+++ b/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();
diff --git a/gazebo/wifi.h b/gazebo/wifi.h
new file mode 100644
index 0000000..fe0a8d5
--- /dev/null
+++ b/gazebo/wifi.h
@@ -0,0 +1,44 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// 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);
+}