Merge branch 'master' into run-sim

.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: |

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

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)

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;
 	}
 }
 

flix/rc.ino

@@ -5,6 +5,8 @@
 
 #include <SBUS.h>
 
+#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() {

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();

gazebo/Arduino.h

@@ -122,6 +122,8 @@ public:
 		}
 		return -1;
 	}
+
+	void setRxInvert(bool invert) {};
 };
 
 HardwareSerial Serial, Serial2;

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)

gazebo/README.md

@@ -0,0 +1,15 @@
+# Gazebo Simulation
+
+<img src="../docs/img/simulator.png" width=500 alt="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.

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();
 	};
 };

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) {};

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;
 }

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]));

gazebo/wifi.h

@@ -0,0 +1,44 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// 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);
+}