From 1b54b3fa2586b3a4e12c705aab5f62085699b945 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 26 Jan 2024 13:16:17 +0300 Subject: [PATCH 1/9] Enable macOS build https://github.com/osrf/homebrew-simulation/pull/2526#issuecomment-1904384070 --- .github/workflows/build.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 5df69c3..9f19958 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -53,7 +53,6 @@ jobs: retention-days: 1 build_simulator_macos: - if: false # temporarily disable runs-on: macos-latest steps: - uses: actions/checkout@v3 From 69cfc9e5fa19ade366543e4d3d194a60a54a3f42 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 26 Jan 2024 13:46:13 +0300 Subject: [PATCH 2/9] Utilize internal ESP32 UART invertor for SBUS --- flix/rc.ino | 3 +++ gazebo/Arduino.h | 2 ++ 2 files changed, 5 insertions(+) diff --git a/flix/rc.ino b/flix/rc.ino index a4b59a5..ce7e439 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -5,6 +5,8 @@ #include +#define INVERT_SERIAL true // false if external SBUS invertor is used + // NOTE: use `cr` command and replace with the actual values int channelNeutral[] = {995, 883, 200, 972, 512, 512}; int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472}; @@ -14,6 +16,7 @@ SBUS RC(Serial2); void setupRC() { Serial.println("Setup RC"); RC.begin(); + Serial2.setRxInvert(INVERT_SERIAL); } void readRC() { diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 9d35e6d..16e0808 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -122,6 +122,8 @@ public: } return -1; } + + void setRxInvert(bool invert) {}; }; HardwareSerial Serial, Serial2; From 6b52ad562b2f329995a6785d034746241cf6b348 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:00:23 +0300 Subject: [PATCH 3/9] Minor const clarification --- gazebo/simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index 9591960..b1ffd46 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -101,7 +101,7 @@ public: body->AddLinkForce(Vector3d(0.0, 0.0, mrr), Vector3d(-dist, -dist, 0.0)); // torque - const double maxTorque = 0.0024; // 24 g*cm ≈ 24 mN*m + const double maxTorque = 0.0024 * ONE_G; // ~24 g*cm body->AddRelativeTorque(Vector3d(0.0, 0.0, scale0 * maxTorque * motors[MOTOR_FRONT_LEFT])); body->AddRelativeTorque(Vector3d(0.0, 0.0, scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT])); body->AddRelativeTorque(Vector3d(0.0, 0.0, scale2 * -maxTorque * motors[MOTOR_REAR_LEFT])); From a24f039f1dfa629bd2f7febe39fc414f69681539 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:04:44 +0300 Subject: [PATCH 4/9] Fix RC_CHANNELS_SCALED inactive channel values They should be INT16_MAX not UINT16_MAX --- flix/mavlink.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 816534e..44e7005 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -44,7 +44,7 @@ void sendMavlink() { mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls[0] * 10000, controls[1] * 10000, controls[2] * 10000, controls[3] * 10000, controls[4] * 10000, controls[5] * 10000, - UINT16_MAX, UINT16_MAX, 255); + INT16_MAX, INT16_MAX, UINT8_MAX); sendMessage(&msg); float actuator[32]; From 033e74a3754d10be08b03ca68746dae545cfce5a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:05:25 +0300 Subject: [PATCH 5/9] Minor code cleanups --- flix/mavlink.ino | 2 +- flix/wifi.ino | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 44e7005..758c90f 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -28,7 +28,7 @@ void sendMavlink() { lastSlow = t; mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, - MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0, + MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0), 0, MAV_STATE_STANDBY); sendMessage(&msg); } diff --git a/flix/wifi.ino b/flix/wifi.ino index 562610e..3572421 100644 --- a/flix/wifi.ino +++ b/flix/wifi.ino @@ -24,7 +24,6 @@ void setupWiFi() { } void sendWiFi(const uint8_t *buf, int len) { -// if (!udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT)) return; udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT); udp.write(buf, len); udp.endPacket(); From 2694f68b876606f50cff6f935e51af41053e84f8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:05:49 +0300 Subject: [PATCH 6/9] Add yaw dead zone in mavlink control --- flix/mavlink.ino | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 758c90f..64bcd07 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -11,6 +11,7 @@ #define PERIOD_SLOW 1.0 #define PERIOD_FAST 0.1 #define MAVLINK_CONTROL_SCALE 0.7f +#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f void processMavlink() { sendMavlink(); @@ -91,6 +92,8 @@ void handleMavlink(const void *_msg) { controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE; controls[RC_CHANNEL_MODE] = 1; // STAB mode controls[RC_CHANNEL_ARMED] = 1; // armed + + if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0; } } From 4850b95029cbf37c1983784c550714e80f3e17d0 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:07:37 +0300 Subject: [PATCH 7/9] Add a readme to gazebo directory --- gazebo/README.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 gazebo/README.md diff --git a/gazebo/README.md b/gazebo/README.md new file mode 100644 index 0000000..3eef4ba --- /dev/null +++ b/gazebo/README.md @@ -0,0 +1,15 @@ +# Gazebo Simulation + +Flix simulator + +## Building and running + +See [building and running instructions](../docs/build.md#simulation). + +## Code structure + +Flix simulator is based on [Gazebo Classic](https://classic.gazebosim.org) and consists of the following components: + +* Physical model of the drone: [`models/flix/flix.sdf`](models/flix/flix.sdf). +* Plugin for Gazebo: [`simulator.cpp`](simulator.cpp). The plugin is attached to the physical model. It receives stick positions from the controller, gets the data from the virtual sensors, and then passes this data to the Arduino code. +* Arduino imitation: [`Arduino.h`](Arduino.h). This file contains partial implementation of the Arduino API, that is working within Gazebo plugin environment. From f718af7f0ed8c2e8a5ffde8d44d4002d5dd53d0f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 12:10:18 +0300 Subject: [PATCH 8/9] Support MAVLink usage in simulation --- .github/workflows/build.yml | 4 ++++ Makefile | 2 +- docs/build.md | 17 +++++++++----- gazebo/CMakeLists.txt | 4 ++++ gazebo/SBUS.h | 3 +-- gazebo/flix.h | 8 +++++++ gazebo/joystick.h | 5 +++-- gazebo/simulator.cpp | 2 ++ gazebo/wifi.h | 44 +++++++++++++++++++++++++++++++++++++ 9 files changed, 78 insertions(+), 11 deletions(-) create mode 100644 gazebo/wifi.h 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); +} From 0f83e8ed8090e7dacb5a5fb00f9d0e491a077efc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jan 2024 17:20:59 +0300 Subject: [PATCH 9/9] Add info on how to control the simulated drone to build instructions --- docs/build.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/build.md b/docs/build.md index 3984451..4a5caf9 100644 --- a/docs/build.md +++ b/docs/build.md @@ -65,6 +65,10 @@ cd flix make simulator ``` +### Flight + +Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. + ## Firmware ### Arduino IDE (Windows, Linux, macOS)