diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 169e582..0d9c0ed 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 @@ -57,6 +59,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 4fedba5..7d976bd 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 GAZEBO ?= gazebo diff --git a/docs/build.md b/docs/build.md index af697a4..4a5caf9 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 ``` @@ -60,6 +65,10 @@ Dependencies are [Gazebo Classic simulator](https://classic.gazebosim.org) and [ 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) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 816534e..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(); @@ -28,7 +29,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); } @@ -44,7 +45,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]; @@ -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; } } 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/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(); 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; 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/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. 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 9591960..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(); @@ -101,7 +103,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])); 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); +}