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/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: | 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 +// Repository: https://github.com/okalachev/flix + +// sendWiFi and receiveWiFi implementations for the simulation + +#include +#include +#include +#include +#include +#include +#include + +#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); +}