Implement parameters subsystem

* Unified parameters storage.
* Store parameters in flash on the hardware.
* Store parameters in text file in simulation.
* Work with parameters in command line.
* Support parameters in MAVLink for working with parameters in QGC.

.vscode/c_cpp_properties.json

@@ -29,7 +29,8 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/util.ino",
-				"${workspaceFolder}/flix/wifi.ino"
+				"${workspaceFolder}/flix/wifi.ino",
+				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -76,7 +77,8 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/util.ino",
-				"${workspaceFolder}/flix/wifi.ino"
+				"${workspaceFolder}/flix/wifi.ino",
+				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++",
 			"cStandard": "c11",
@@ -123,7 +125,8 @@
 				"${workspaceFolder}/flix/rc.ino",
 				"${workspaceFolder}/flix/time.ino",
 				"${workspaceFolder}/flix/util.ino",
-				"${workspaceFolder}/flix/wifi.ino"
+				"${workspaceFolder}/flix/wifi.ino",
+				"${workspaceFolder}/flix/parameters.ino"
 			],
 			"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2302/bin/xtensa-esp32-elf-g++.exe",
 			"cStandard": "c11",

docs/build.md

@@ -143,8 +143,7 @@ See other available Make commands in the [Makefile](../Makefile).
 Before flight you need to calibrate the accelerometer:
 
 1. Open Serial Monitor in Arduino IDE (use use `make monitor` command in the command line).
-2. Type `ca` command there.
-3. Copy calibration results to the source code (`flix/imu.ino`).
+2. Type `ca` command there and follow the instructions.
 
 #### Control with smartphone
 
@@ -160,8 +159,7 @@ Before flight you need to calibrate the accelerometer:
 Before flight using remote control, you need to calibrate it:
 
 1. Open Serial Monitor in Arduino IDE (use use `make monitor` command in the command line).
-2. Type `cr` command there.
-3. Copy calibration results to the source code (`flix/rc.ino`).
+2. Type `cr` command there and follow the instructions.
 
 Then you can use your remote control to fly the drone!
 

docs/troubleshooting.md

@@ -19,7 +19,7 @@ Do the following:
 * **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while.
 * **Check the IMU sample rate**. Perform `imu` command. The `rate` field should be about 1000 (Hz).
 * **Check the IMU data**. Perform `imu` command, check raw accelerometer and gyro output. The output should change as you move the drone.
-* **Calibrate the accelerometer.** if is wasn't done before. Perform `ca` command and put the results to `imu.ino` file.
+* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
 * **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
 * **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, make sure `rotateIMU` function is defined correctly in `imu.ino` file.
 * **Check the motors**. Perform the following commands using Serial Monitor:
@@ -27,7 +27,7 @@ Do the following:
   * `mfl` — should rotate front left motor (clockwise).
   * `mrl` — should rotate rear left motor (counter-clockwise).
   * `mrr` — should rotate rear right motor (clockwise).
-* **Calibrate the RC** if you use it. Perform `cr` command and put the results to `rc.ino` file.
+* **Calibrate the RC** if you use it. Type `cr` command in Serial Monitor and follow the instructions.
 * **Check the RC data** if you use it. Use `rc` command, `Control` should show correct values between -1 and 1, and between 0 and 1 for the throttle.
 * **Check the IMU output using QGroundControl**. Connect to the drone using QGroundControl on your computer. Go to the *Analyze* tab, *MAVLINK Inspector*. Plot the data from the `SCALED_IMU` message. The gyroscope and accelerometer data should change according to the drone movement.
 * **Check the gyroscope only attitude estimation**. Comment out `applyAcc();` line in `estimate.ino` and check if the attitude estimation in QGroundControl. It should be stable, but only drift very slowly.

flix/cli.ino

@@ -19,8 +19,10 @@ const char* motd =
 "|__|     |_______||__| /__/ \\__\\\n\n"
 "Commands:\n\n"
 "help - show help\n"
-"show - show all parameters\n"
-"<name> <value> - set parameter\n"
+"p - show all parameters\n"
+"p <name> - show parameter\n"
+"p <name> <value> - set parameter\n"
+"preset - reset parameters\n"
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
@@ -34,36 +36,22 @@ const char* motd =
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
 
-const struct Param {
-	const char* name;
-	float* value;
-	float* value2;
-} params[] = {
-	{"rp", &rollRatePID.p, &pitchRatePID.p},
-	{"ri", &rollRatePID.i, &pitchRatePID.i},
-	{"rd", &rollRatePID.d, &pitchRatePID.d},
-
-	{"ap", &rollPID.p, &pitchPID.p},
-	{"ai", &rollPID.i, &pitchPID.i},
-	{"ad", &rollPID.d, &pitchPID.d},
-
-	{"yp", &yawRatePID.p, nullptr},
-	{"yi", &yawRatePID.i, nullptr},
-	{"yd", &yawRatePID.d, nullptr},
-
-	{"lpr", &ratesFilter.alpha, nullptr},
-	{"lpd", &rollRatePID.lpf.alpha, &pitchRatePID.lpf.alpha},
-
-	{"ss", &loopRate, nullptr},
-	{"dt", &dt, nullptr},
-	{"t", &t, nullptr},
-};
-
-void doCommand(String& command, String& value) {
+void doCommand(String& command, String& arg0, String& arg1) {
 	if (command == "help" || command == "motd") {
 		Serial.println(motd);
-	} else if (command == "show") {
-		showTable();
+	} else if (command == "p" && arg0 == "") {
+		printParameters();
+	} else if (command == "p" && arg0 != "" && arg1 == "") {
+		Serial.printf("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
+	} else if (command == "p") {
+		bool success = setParameter(arg0.c_str(), arg1.toFloat());
+		if (success) {
+			Serial.printf("%s = %g\n", arg0.c_str(), arg1.toFloat());
+		} else {
+			Serial.printf("Parameter not found: %s\n", arg0.c_str());
+		}
+	} else if (command == "preset") {
+		resetParameters();
 	} else if (command == "ps") {
 		Vector a = attitude.toEulerZYX();
 		Serial.printf("roll: %f pitch: %f yaw: %f\n", a.x * RAD_TO_DEG, a.y * RAD_TO_DEG, a.z * RAD_TO_DEG);
@@ -106,32 +94,13 @@ void doCommand(String& command, String& value) {
 		attitude = Quaternion();
 	} else if (command == "reboot") {
 		ESP.restart();
+	} else if (command == "") {
+		// do nothing
 	} else {
-		float val = value.toFloat();
-		// TODO: on error returns 0, check invalid value
-
-		for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
-			if (command == params[i].name) {
-				*params[i].value = val;
-				if (params[i].value2 != nullptr) *params[i].value2 = val;
-				Serial.print(command);
-				Serial.print(" = ");
-				Serial.println(val, 4);
-				return;
-			}
-		}
 		Serial.println("Invalid command: " + command);
 	}
 }
 
-void showTable() {
-	for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
-		Serial.print(params[i].name);
-		Serial.print(" ");
-		Serial.println(*params[i].value, 5);
-	}
-}
-
 void cliTestMotor(uint8_t n) {
 	Serial.printf("Testing motor %d\n", n);
 	motors[n] = 1;
@@ -145,9 +114,7 @@ void cliTestMotor(uint8_t n) {
 
 void parseInput() {
 	static bool showMotd = true;
-	static String command;
-	static String value;
-	static bool parsingCommand = true;
+	static String input;
 
 	if (showMotd) {
 		Serial.println(motd);
@@ -157,16 +124,21 @@ void parseInput() {
 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			parsingCommand = true;
-			if (!command.isEmpty()) {
-				doCommand(command, value);
-			}
-			command.clear();
-			value.clear();
-		} else if (c == ' ') {
-			parsingCommand = false;
+			char chars[input.length() + 1];
+			input.toCharArray(chars, input.length() + 1);
+			String command = stringToken(chars, " ");
+			String arg0 = stringToken(NULL, " ");
+			String arg1 = stringToken(NULL, "");
+			doCommand(command, arg0, arg1);
+			input.clear();
 		} else {
-			(parsingCommand ? command : value) += c;
+			input += c;
 		}
 	}
 }
+
+// Helper function for parsing input
+String stringToken(char* str, const char* delim) {
+	char* token = strtok(str, delim);
+	return token == NULL ? "" : token;
+}

flix/flix.ino

@@ -41,6 +41,7 @@ void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
 	Serial.println("Initializing flix");
 	disableBrownOut();
+	setupParameters();
 	setupLED();
 	setupMotors();
 	setLED(true);
@@ -66,4 +67,5 @@ void loop() {
 	processMavlink();
 #endif
 	logData();
+	flushParameters();
 }

flix/imu.ino

@@ -6,12 +6,11 @@
 #include <SPI.h>
 #include <MPU9250.h>
 
-// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
-Vector accBias(0, 0, 0);
-Vector accScale(1, 1, 1);
-
 MPU9250 IMU(SPI);
+
+Vector accBias;
 Vector gyroBias;
+Vector accScale(1, 1, 1);
 
 void setupIMU() {
 	Serial.println("Setup IMU");

flix/mavlink.ino

@@ -84,6 +84,7 @@ void receiveMavlink() {
 
 void handleMavlink(const void *_msg) {
 	mavlink_message_t *msg = (mavlink_message_t *)_msg;
+
 	if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
 		mavlink_manual_control_t manualControl;
 		mavlink_msg_manual_control_decode(msg, &manualControl);
@@ -97,6 +98,49 @@ void handleMavlink(const void *_msg) {
 
 		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
 	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
+		mavlink_message_t msg;
+		for (int i = 0; i < parametersCount(); i++) {
+			mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+				getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
+			sendMessage(&msg);
+		}
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
+		mavlink_param_request_read_t paramRequestRead;
+		mavlink_msg_param_request_read_decode(msg, &paramRequestRead);
+		char name[16 + 1];
+		strlcpy(name, paramRequestRead.param_id, sizeof(name)); // param_id might be not null-terminated
+		float value = strlen(name) == 0 ? getParameter(paramRequestRead.param_index) : getParameter(name);
+		if (paramRequestRead.param_index != -1) {
+			memcpy(name, getParameterName(paramRequestRead.param_index), 16);
+		}
+		mavlink_message_t msg;
+		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), paramRequestRead.param_index);
+		sendMessage(&msg);
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+		mavlink_param_set_t paramSet;
+		mavlink_msg_param_set_decode(msg, &paramSet);
+		char name[16 + 1];
+		strlcpy(name, paramSet.param_id, sizeof(name)); // param_id might be not null-terminated
+		setParameter(name, paramSet.param_value);
+		// send ack
+		mavlink_message_t msg;
+		mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			paramSet.param_id, paramSet.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
+		sendMessage(&msg);
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
+		mavlink_message_t msg;
+		mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
+		sendMessage(&msg);
+	}
 }
 
 // Convert Forward-Left-Up to Forward-Right-Down quaternion

flix/parameters.ino

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <Preferences.h>
+#include <vector>
+
+extern float channelNeutral[RC_CHANNELS];
+extern float channelMax[RC_CHANNELS];
+
+Preferences storage;
+
+struct Parameter {
+	const char *name;
+	float *variable;
+	float value; // cache
+};
+
+Parameter parameters[] = {
+	// control
+	{"ROLLRATE_P", &rollRatePID.p},
+	{"ROLLRATE_I", &rollRatePID.i},
+	{"ROLLRATE_D", &rollRatePID.d},
+	{"ROLLRATE_I_LIM", &rollRatePID.windup},
+	{"PITCHRATE_P", &pitchRatePID.p},
+	{"PITCHRATE_I", &pitchRatePID.i},
+	{"PITCHRATE_D", &pitchRatePID.d},
+	{"PITCHRATE_I_LIM", &pitchRatePID.windup},
+	{"YAWRATE_P", &yawRatePID.p},
+	{"YAWRATE_I", &yawRatePID.i},
+	{"YAWRATE_D", &yawRatePID.d},
+	{"ROLL_P", &rollPID.p},
+	{"ROLL_I", &rollPID.i},
+	{"ROLL_D", &rollPID.d},
+	{"PITCH_P", &pitchPID.p},
+	{"PITCH_I", &pitchPID.i},
+	{"PITCH_D", &pitchPID.d},
+	{"YAW_P", &yawPID.p},
+	// imu
+	{"ACC_BIAS_X", &accBias.x},
+	{"ACC_BIAS_Y", &accBias.y},
+	{"ACC_BIAS_Z", &accBias.z},
+	{"ACC_SCALE_X", &accScale.x},
+	{"ACC_SCALE_Y", &accScale.y},
+	{"ACC_SCALE_Z", &accScale.z},
+	{"GYRO_BIAS_X", &gyroBias.x},
+	{"GYRO_BIAS_Y", &gyroBias.y},
+	{"GYRO_BIAS_Z", &gyroBias.z},
+	// rc
+	{"RC_NEUTRAL_0", &channelNeutral[0]},
+	{"RC_NEUTRAL_1", &channelNeutral[1]},
+	{"RC_NEUTRAL_2", &channelNeutral[2]},
+	{"RC_NEUTRAL_3", &channelNeutral[3]},
+	{"RC_NEUTRAL_4", &channelNeutral[4]},
+	{"RC_NEUTRAL_5", &channelNeutral[5]},
+	{"RC_NEUTRAL_6", &channelNeutral[6]},
+	{"RC_NEUTRAL_7", &channelNeutral[7]},
+	{"RC_MAX_0", &channelMax[0]},
+	{"RC_MAX_1", &channelMax[1]},
+	{"RC_MAX_2", &channelMax[2]},
+	{"RC_MAX_3", &channelMax[3]},
+	{"RC_MAX_4", &channelMax[4]},
+	{"RC_MAX_5", &channelMax[5]},
+	{"RC_MAX_6", &channelMax[6]},
+	{"RC_MAX_7", &channelMax[7]}
+};
+
+void setupParameters() {
+	storage.begin("flix", false);
+	// Read parameters from storage
+	for (auto &parameter : parameters) {
+		if (!storage.isKey(parameter.name)) {
+			Serial.printf("Define new parameter %s = %f\n", parameter.name, *parameter.variable);
+			storage.putFloat(parameter.name, *parameter.variable);
+		}
+		*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
+		parameter.value = *parameter.variable;
+	}
+}
+
+int parametersCount() {
+	return sizeof(parameters) / sizeof(parameters[0]);
+}
+
+const char *getParameterName(int index) {
+	return parameters[index].name;
+}
+
+float getParameter(int index) {
+	return *parameters[index].variable;
+}
+
+float getParameter(const char *name) {
+	for (auto &parameter : parameters) {
+		if (strcmp(parameter.name, name) == 0) {
+			return *parameter.variable;
+		}
+	}
+	return NAN;
+}
+
+bool setParameter(const char *name, const float value) {
+	for (auto &parameter : parameters) {
+		if (strcmp(parameter.name, name) == 0) {
+			*parameter.variable = value;
+			return true;
+		}
+	}
+	return false;
+}
+
+void flushParameters() {
+	static float lastFlush = 0;
+	if (t - lastFlush < 1) return; // flush once per second
+	if (motorsActive()) return; // don't use flash while flying, it may cause a delay
+	lastFlush = t;
+
+	for (auto &parameter : parameters) {
+		if (parameter.value == *parameter.variable) continue;
+		if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
+		storage.putFloat(parameter.name, *parameter.variable);
+		parameter.value = *parameter.variable;
+	}
+}
+
+void printParameters() {
+	for (auto &parameter : parameters) {
+		Serial.printf("%s = %g\n", parameter.name, *parameter.variable);
+	}
+}
+
+void resetParameters() {
+	storage.clear();
+	ESP.restart();
+}

flix/rc.ino

@@ -5,9 +5,8 @@
 
 #include <SBUS.h>
 
-// NOTE: use 'cr' command to calibrate the RC and put the values here
-int channelNeutral[] = {995, 883, 200, 972, 512, 512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+float channelNeutral[RC_CHANNELS] = {NAN}; // first element NAN means not calibrated
+float channelMax[RC_CHANNELS];
 
 SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
@@ -26,6 +25,7 @@ void readRC() {
 }
 
 void normalizeRC() {
+	if (isnan(channelNeutral[0])) return; // skip if not calibrated
 	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
 		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
 	}

gazebo/Arduino.h

@@ -27,15 +27,27 @@ long map(long x, long in_min, long in_max, long out_min, long out_max) {
 	return (delta * rise) / run + out_min;
 }
 
+size_t strlcpy(char* dst, const char* src, size_t len) {
+	size_t l = strlen(src);
+	size_t i = 0;
+	while (i < len - 1 && *src != '\0') { *dst++ = *src++; i++; }
+	*dst = '\0';
+	return l;
+}
+
 class __FlashStringHelper;
 
 // Arduino String partial implementation
 // https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
 class String: public std::string {
 public:
+	String(const char *str = "") : std::string(str) {}
 	long toInt() const { return atol(this->c_str()); }
 	float toFloat() const { return atof(this->c_str()); }
 	bool isEmpty() const { return this->empty(); }
+	void toCharArray(char *buf, unsigned int bufsize, unsigned int index = 0) const {
+		strlcpy(buf, this->c_str() + index, bufsize);
+	}
 };
 
 class Print;

gazebo/Preferences.h

@@ -0,0 +1,63 @@
+// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Partial implementation of the ESP32 Preferences library for the simulation
+
+#include <map>
+#include <fstream>
+#include "util.h"
+
+class Preferences {
+private:
+	std::map<std::string, float> storage;
+	std::string storagePath;
+
+	void readFromFile() {
+		std::ifstream file(storagePath);
+		std::string key;
+		float value;
+		while (file >> key >> value) {
+			storage[key] = value;
+		}
+	}
+
+	void writeToFile() {
+		std::ofstream file(storagePath);
+		for (auto &pair : storage) {
+			file << pair.first << " " << pair.second << std::endl;
+		}
+	}
+
+public:
+	bool begin(const char *name, bool readOnly = false, const char *partition_label = NULL) {
+		storagePath = getPluginPath().parent_path() / (std::string(name) + ".txt");
+		gzmsg << "Preferences initialized: " << storagePath << std::endl;
+		readFromFile();
+		return true;
+	}
+
+	void end();
+
+	bool isKey(const char *key) {
+		return storage.find(key) != storage.end();
+	}
+
+	size_t putFloat(const char *key, float value) {
+		storage[key] = value;
+		writeToFile();
+		return sizeof(value);
+	}
+
+	float getFloat(const char *key, float defaultValue = NAN) {
+		if (!isKey(key)) {
+			return defaultValue;
+		}
+		return storage[key];
+	}
+
+	bool clear() {
+		storage.clear();
+		writeToFile();
+		return true;
+	}
+};

gazebo/flix.h

@@ -45,6 +45,8 @@ void controlTorque();
 void showTable();
 bool motorsActive();
 void cliTestMotor(uint8_t n);
+String stringToken(char* str, const char* delim);
+void normalizeRC();
 void printRCCal();
 void processMavlink();
 void sendMavlink();
@@ -62,3 +64,4 @@ void calibrateAccel() { printf("Skip accel calibrating\n"); };
 void sendMotors() {};
 void printIMUCal() { printf("cal: N/A\n"); };
 void printIMUInfo() {};
+Vector accBias, gyroBias, accScale(1, 1, 1);

gazebo/joystick.h

@@ -7,10 +7,6 @@
 #include <gazebo/gazebo.hh>
 #include <iostream>
 
-// simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
-const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
-
 #define RC_CHANNEL_ROLL 0
 #define RC_CHANNEL_PITCH 1
 #define RC_CHANNEL_THROTTLE 2
@@ -21,8 +17,6 @@ const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
 SDL_Joystick *joystick;
 bool joystickInitialized = false, warnShown = false;
 
-void normalizeRC();
-
 void joystickInit() {
 	SDL_Init(SDL_INIT_JOYSTICK);
 	joystick = SDL_JoystickOpen(0);
@@ -33,12 +27,6 @@ void joystickInit() {
 		gzwarn << "Joystick not found, begin waiting for joystick..." << std::endl;
 		warnShown = true;
 	}
-
-	// apply calibration overrides
-	extern int channelNeutral[RC_CHANNELS];
-	extern int channelMax[RC_CHANNELS];
-	memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
-	memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
 }
 
 bool joystickGet(int16_t ch[16]) {

gazebo/simulator.cpp

@@ -23,6 +23,7 @@
 #include "estimate.ino"
 #include "control.ino"
 #include "log.ino"
+#include "parameters.ino"
 #include "cli.ino"
 #include "mavlink.ino"
 #include "failsafe.ino"
@@ -51,6 +52,7 @@ public:
 		this->resetConnection = event::Events::ConnectWorldReset(std::bind(&ModelFlix::OnReset, this));
 		initNode();
 		Serial.begin(0);
+		setupParameters();
 		gzmsg << "Flix plugin loaded" << endl;
 	}
 
@@ -85,6 +87,7 @@ public:
 		applyMotorForces();
 		publishTopics();
 		logData();
+		flushParameters();
 	}
 
 	void applyMotorForces() {

gazebo/util.h

@@ -0,0 +1,9 @@
+#include <filesystem>
+
+std::filesystem::path getPluginPath() {
+	Dl_info dl_info;
+	if (dladdr((void*)&getPluginPath, &dl_info) == 0) {
+		throw std::runtime_error("Unable to determine plugin path using dladdr.");
+	}
+	return std::filesystem::path(dl_info.dli_fname);
+}