Add parameters for mavlink subsystem

System ID, fast telemetry rate, slow telemetry rate.

.vscode/c_cpp_properties.json

@@ -6,18 +6,19 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
+				"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Arduino/libraries/**",
 				"/usr/include/gazebo-11/",
 				"/usr/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
+				"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -30,10 +31,9 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
+				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -53,18 +53,19 @@
 			"name": "Mac",
 			"includePath": [
 				"${workspaceFolder}/flix",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
+				"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**",
 				"/opt/homebrew/include/gazebo-11/",
 				"/opt/homebrew/include/ignition/math6/"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
+				"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/flix.ino",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
@@ -77,10 +78,9 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
+				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
+			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [
@@ -103,16 +103,17 @@
 				"${workspaceFolder}/flix",
 				"${workspaceFolder}/gazebo",
 				"${workspaceFolder}/tools/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
-				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
+				"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
 				"~/Documents/Arduino/libraries/**"
 			],
 			"forcedInclude": [
 				"${workspaceFolder}/.vscode/intellisense.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
-				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
+				"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
 				"${workspaceFolder}/flix/cli.ino",
 				"${workspaceFolder}/flix/control.ino",
 				"${workspaceFolder}/flix/estimate.ino",
@@ -125,10 +126,9 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/wifi.ino",
-				"${workspaceFolder}/flix/parameters.ino",
-				"${workspaceFolder}/flix/safety.ino"
+				"${workspaceFolder}/flix/parameters.ino"
 			],
-			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
+			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",
 			"cppStandard": "c++17",
 			"defines": [

Makefile

@@ -13,10 +13,10 @@ monitor:
 
 dependencies .dependencies:
 	arduino-cli core update-index --config-file arduino-cli.yaml
-	arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
+	arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
 	arduino-cli lib update-index
 	arduino-cli lib install "FlixPeriph"
-	arduino-cli lib install "MAVLink"@2.0.25
+	arduino-cli lib install "MAVLink"@2.0.16
 	touch .dependencies
 
 gazebo/build cmake: gazebo/CMakeLists.txt

docs/troubleshooting.md

@@ -16,7 +16,7 @@ Do the following:
 * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
 * **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
 * **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
-* **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
+* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
 * **Check the IMU is working**. Perform `imu` command and check its output:

docs/usage.md

@@ -20,10 +20,10 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
 
 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
 2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
-3. Install ESP32 core, version 3.3.6. 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 ESP32 core, version 3.2.0. 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.
 4. 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.25.
+   * `MAVLink`, version 2.0.16.
 5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
 6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
 7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
@@ -108,7 +108,7 @@ The drone is configured using parameters. To access and modify them, go to the Q
 
 <img src="img/parameters.png" width="400">
 
-You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
+You can also work with parameters using `p` command in the console.
 
 ### Define IMU orientation
 
@@ -271,12 +271,6 @@ ap my-flix-ssid mypassword123
 p WIFI_MODE 1
 ```
 
-Disabling Wi-Fi:
-
-```
-p WIFI_MODE 0
-```
-
 ## Flight log
 
 After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:

flix/cli.ino

@@ -6,7 +6,6 @@
 #include "pid.h"
 #include "vector.h"
 #include "util.h"
-#include "lpf.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern const int RAW, ACRO, STAB, AUTO;
@@ -15,7 +14,6 @@ extern uint16_t channels[16];
 extern float controlTime;
 extern int mode;
 extern bool armed;
-extern LowPassFilter<Vector> gyroBiasFilter;
 
 const char* motd =
 "\nWelcome to\n"
@@ -180,7 +178,6 @@ void doCommand(String str, bool echo = false) {
 #endif
 	} else if (command == "reset") {
 		attitude = Quaternion();
-		gyroBiasFilter.reset();
 	} else if (command == "reboot") {
 		ESP.restart();
 	} else {

flix/flix.ino

@@ -7,6 +7,7 @@
 #include "quaternion.h"
 #include "util.h"
 
+
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
 extern Vector gyro, acc;

flix/imu.ino

@@ -19,8 +19,6 @@ Vector acc; // accelerometer output, m/s/s
 Vector accBias;
 Vector accScale(1, 1, 1);
 
-LowPassFilter<Vector> gyroBiasFilter(0.001);
-
 void setupIMU() {
 	print("Setup IMU\n");
 	imu.begin();
@@ -52,6 +50,8 @@ void readIMU() {
 void calibrateGyroOnce() {
 	static Delay landedDelay(2);
 	if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
+
+	static LowPassFilter<Vector> gyroBiasFilter(0.001);
 	gyroBias = gyroBiasFilter.update(gyro);
 }
 

flix/lpf.h

@@ -14,6 +14,15 @@ public:
 	LowPassFilter(float alpha): alpha(alpha) {};
 
 	T update(const T input) {
+		if (alpha == 1) { // filter disabled
+			return input;
+		}
+
+		if (!initialized) {
+			output = input;
+			initialized = true;
+		}
+
 		return output += alpha * (input - output);
 	}
 
@@ -22,6 +31,9 @@ public:
 	}
 
 	void reset() {
-		output = T(); // set to zero
+		initialized = false;
 	}
+
+private:
+	bool initialized = false;
 };

flix/mavlink.ino

@@ -142,7 +142,7 @@ void handleMavlink(const void *_msg) {
 		// send ack
 		mavlink_message_t msg;
 		mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
-			m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
+			m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
 		sendMessage(&msg);
 	}
 

flix/parameters.ino

@@ -10,7 +10,6 @@ extern float channelZero[16];
 extern float channelMax[16];
 extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int wifiMode, udpLocalPort, udpRemotePort;
-extern float rcLossTimeout, descendTime;
 
 Preferences storage;
 
@@ -59,7 +58,6 @@ Parameter parameters[] = {
 	{"IMU_ACC_SCALE_X", &accScale.x},
 	{"IMU_ACC_SCALE_Y", &accScale.y},
 	{"IMU_ACC_SCALE_Z", &accScale.z},
-	{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
 	// estimate
 	{"EST_ACC_WEIGHT", &accWeight},
 	{"EST_RATES_LPF_A", &ratesFilter.alpha},
@@ -93,9 +91,6 @@ Parameter parameters[] = {
 	{"MAV_SYS_ID", &mavlinkSysId},
 	{"MAV_RATE_SLOW", &telemetrySlow.rate},
 	{"MAV_RATE_FAST", &telemetryFast.rate},
-	// safety
-	{"SF_RC_LOSS_TIME", &rcLossTimeout},
-	{"SF_DESCEND_TIME", &descendTime},
 };
 
 void setupParameters() {
@@ -126,7 +121,7 @@ float getParameter(int index) {
 
 float getParameter(const char *name) {
 	for (auto &parameter : parameters) {
-		if (strcasecmp(parameter.name, name) == 0) {
+		if (strcmp(parameter.name, name) == 0) {
 			return parameter.getValue();
 		}
 	}
@@ -135,7 +130,7 @@ float getParameter(const char *name) {
 
 bool setParameter(const char *name, const float value) {
 	for (auto &parameter : parameters) {
-		if (strcasecmp(parameter.name, name) == 0) {
+		if (strcmp(parameter.name, name) == 0) {
 			if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
 			parameter.setValue(value);
 			return true;

flix/rc.ino

@@ -13,10 +13,10 @@ float channelZero[16]; // calibration zero values
 float channelMax[16]; // calibration max values
 
 float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
-float controlMode = NAN;
-float controlTime = NAN; // time of the last controls update
+float controlMode = NAN; //
+float controlTime; // time of the last controls update (0 when no RC)
 
-// Channels mapping (nan means not assigned):
+// Channels mapping (using float to store in parameters):
 float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
 
 void setupRC() {

flix/safety.ino

@@ -3,12 +3,12 @@
 
 // Fail-safe functions
 
+#define RC_LOSS_TIMEOUT 1
+#define DESCEND_TIME 10
+
 extern float controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
-float rcLossTimeout = 1;
-float descendTime = 10;
-
 void failsafe() {
 	rcLossFailsafe();
 	autoFailsafe();
@@ -16,8 +16,9 @@ void failsafe() {
 
 // RC loss failsafe
 void rcLossFailsafe() {
+	if (controlTime == 0) return; // no RC at all
 	if (!armed) return;
-	if (t - controlTime > rcLossTimeout) {
+	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
@@ -26,7 +27,7 @@ void rcLossFailsafe() {
 void descend() {
 	mode = AUTO;
 	attitudeTarget = Quaternion();
-	thrustTarget -= dt / descendTime;
+	thrustTarget -= dt / DESCEND_TIME;
 	if (thrustTarget < 0) {
 		thrustTarget = 0;
 		armed = false;

flix/wifi.ino

@@ -1,7 +1,7 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Wi-Fi communication
+// Wi-Fi connectivity
 
 #include <WiFi.h>
 #include <WiFiAP.h>

tools/grab_log.py

@@ -13,7 +13,7 @@ lines = []
 
 print('Downloading log...')
 count = 0
-dev.write('log dump\n'.encode())
+dev.write('log\n'.encode())
 while True:
     line = dev.readline()
     if not line:

tools/log.py

@@ -43,7 +43,6 @@ records = [record for record in records if record[0] != 0]
 
 print(f'Received records: {len(records)}')
 
-os.makedirs(f'{DIR}/log', exist_ok=True)
 log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
 log.write(header.encode() + b'\n')
 for record in records: