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.

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