Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Support MAVLink usage in simulation
Oleg Kalachevf718af7f0ed8
diff --git a/.github/workflows/build.yml b/.github/workflows/build.ymlindex 9f19958..704b6f1 100644--- a/.github/workflows/build.yml+++ b/.github/workflows/build.yml@@ -39,6 +39,8 @@ jobs:build_simulator:runs-on: ubuntu-lateststeps:+ - name: Install Arduino CLI+ uses: arduino/[email protected]- uses: actions/checkout@v3- name: Install Gazeborun: curl -sSL http://get.gazebosim.org | sh@@ -55,6 +57,8 @@ jobs:build_simulator_macos:runs-on: macos-lateststeps:+ - 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/577run: |diff --git a/Makefile b/Makefileindex c6e2584..58e5f7d 100644--- a/Makefile+++ b/Makefile@@ -24,7 +24,7 @@ gazebo/build cmake: gazebo/CMakeLists.txtmkdir -p gazebo/buildcd gazebo/build && cmake ..-build_simulator: gazebo/build+build_simulator: .dependencies gazebo/buildmake -C gazebo/buildsimulator: build_simulatordiff --git a/docs/build.md b/docs/build.mdindex 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:```bashcurl -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:```bashsudo apt-get update && sudo apt-get install build-essential libsdl2-dev```-3. Run the simulation:+4. Run the simulation:```bashmake 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:```bashbrew tap osrf/simulation+ brew install arduino-clibrew install gazebo11brew install sdl2```diff --git a/gazebo/CMakeLists.txt b/gazebo/CMakeLists.txtindex 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.hindex 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.hindex 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);// mocksvoid setLED(bool on) {};diff --git a/gazebo/joystick.h b/gazebo/joystick.hindex 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.cppindex 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.hnew file mode 100644index 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);+}