Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.
Oleg Kalachevae349fb73c19
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.jsonindex cadd43e..d1ffb29 100644--- a/.vscode/c_cpp_properties.json+++ b/.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",diff --git a/docs/build.md b/docs/build.mdindex e3a4f9c..37bfb16 100644--- a/docs/build.md+++ b/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!diff --git a/docs/troubleshooting.md b/docs/troubleshooting.mdindex 7b493fd..3cce439 100644--- a/docs/troubleshooting.md+++ b/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.diff --git a/flix/cli.ino b/flix/cli.inoindex 1d105fc..58dd54c 100644--- a/flix/cli.ino+++ b/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;+}diff --git a/flix/flix.ino b/flix/flix.inoindex 34335c2..c5de571 100644--- a/flix/flix.ino+++ b/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();#endiflogData();+ flushParameters();}diff --git a/flix/imu.ino b/flix/imu.inoindex 5475e9e..46c526a 100644--- a/flix/imu.ino+++ b/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");diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 3bf73a1..aada8ea 100644--- a/flix/mavlink.ino+++ b/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, ¶mRequestRead);+ 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, ¶mSet);+ 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 quaterniondiff --git a/flix/parameters.ino b/flix/parameters.inonew file mode 100644index 0000000..6b9e688--- /dev/null+++ b/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 ¶meter : 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 ¶meter : parameters) {+ if (strcmp(parameter.name, name) == 0) {+ return *parameter.variable;+ }+ }+ return NAN;+}++bool setParameter(const char *name, const float value) {+ for (auto ¶meter : 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 ¶meter : 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 ¶meter : parameters) {+ Serial.printf("%s = %g\n", parameter.name, *parameter.variable);+ }+}++void resetParameters() {+ storage.clear();+ ESP.restart();+}diff --git a/flix/rc.ino b/flix/rc.inoindex 7af6e25..e570f5f 100644--- a/flix/rc.ino+++ b/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 calibratedfor (uint8_t i = 0; i < RC_CHANNELS; i++) {controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);}diff --git a/gazebo/Arduino.h b/gazebo/Arduino.hindex e065992..86b9870 100644--- a/gazebo/Arduino.h+++ b/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;diff --git a/gazebo/Preferences.h b/gazebo/Preferences.hnew file mode 100644index 0000000..30cb75d--- /dev/null+++ b/gazebo/Preferences.h@@ -0,0 +1,63 @@+// Copyright (c) 2024 Oleg Kalachev <[email protected]>+// 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;+ }+};diff --git a/gazebo/flix.h b/gazebo/flix.hindex 26cbbec..c66f2d7 100644--- a/gazebo/flix.h+++ b/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);diff --git a/gazebo/joystick.h b/gazebo/joystick.hindex 088e6d4..8c8ba84 100644--- a/gazebo/joystick.h+++ b/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]) {diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex 6684c6b..235ff72 100644--- a/gazebo/simulator.cpp+++ b/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() {diff --git a/gazebo/util.h b/gazebo/util.hnew file mode 100644index 0000000..404b408--- /dev/null+++ b/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);+}