diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index d1ffb29..0de0b76 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -5,18 +5,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/**",
-				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/**",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/dio_qspi/include",
 				"~/Arduino/libraries/**",
 				"/usr/include/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32/Arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -32,7 +32,7 @@
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2405/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -53,18 +53,18 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/include/**",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/include/**",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**",
 				"/opt/homebrew/include/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32/Arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/flix.ino",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
@@ -80,7 +80,7 @@
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2405/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -102,17 +102,17 @@
 			"includePath": [
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/**",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.1-632e0c2a/esp32/dio_qspi/include",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/libraries/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/**",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.3-083aad99-v2/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.0.7/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/cores/esp32/Arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.1.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -128,7 +128,7 @@
 				"${workspaceFolder}/flix/wifi.ino",
 				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++.exe",
+			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2405/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
diff --git a/Makefile b/Makefile
index 0da2e4f..c3670eb 100644
--- a/Makefile
+++ b/Makefile
@@ -13,7 +13,7 @@ monitor:
 
 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.0.7 --config-file arduino-cli.yaml
+	arduino-cli core install esp32:esp32@3.1.0 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
 	arduino-cli lib install "MAVLink"@2.0.12
diff --git a/docs/build.md b/docs/build.md
index ee5a304..694283d 100644
--- a/docs/build.md
+++ b/docs/build.md
@@ -105,7 +105,7 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you h
 ### Arduino IDE (Windows, Linux, macOS)
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
-2. Install ESP32 core, version 3.0.7 (version 2.x is not supported). See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
+2. Install ESP32 core, version 3.1.0 (version 2.x is not supported). See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
 3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
    * `FlixPeriph`, the latest version.
    * `MAVLink`, version 2.0.12.