From ae349fb73c191ab9971eba9b442fa350fa6c6566 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 23 Dec 2024 13:00:02 +0300 Subject: [PATCH] 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 | 9 ++- docs/build.md | 6 +- docs/troubleshooting.md | 4 +- flix/cli.ino | 98 +++++++++---------------- flix/flix.ino | 2 + flix/imu.ino | 7 +- flix/mavlink.ino | 44 +++++++++++ flix/parameters.ino | 133 ++++++++++++++++++++++++++++++++++ flix/rc.ino | 6 +- gazebo/Arduino.h | 12 +++ gazebo/Preferences.h | 63 ++++++++++++++++ gazebo/flix.h | 3 + gazebo/joystick.h | 12 --- gazebo/simulator.cpp | 3 + gazebo/util.h | 9 +++ 15 files changed, 320 insertions(+), 91 deletions(-) create mode 100644 flix/parameters.ino create mode 100644 gazebo/Preferences.h create mode 100644 gazebo/util.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 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.md index 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.md index 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.ino index 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" -" - set parameter\n" +"p - show all parameters\n" +"p - show parameter\n" +"p - 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.ino index 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(); #endif logData(); + flushParameters(); } diff --git a/flix/imu.ino b/flix/imu.ino index 5475e9e..46c526a 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -6,12 +6,11 @@ #include #include -// 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.ino index 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 quaternion diff --git a/flix/parameters.ino b/flix/parameters.ino new file mode 100644 index 0000000..6b9e688 --- /dev/null +++ b/flix/parameters.ino @@ -0,0 +1,133 @@ +#pragma once + +#include +#include + +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.ino index 7af6e25..e570f5f 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -5,9 +5,8 @@ #include -// 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); } diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 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.h new file mode 100644 index 0000000..30cb75d --- /dev/null +++ b/gazebo/Preferences.h @@ -0,0 +1,63 @@ +// Copyright (c) 2024 Oleg Kalachev +// Repository: https://github.com/okalachev/flix + +// Partial implementation of the ESP32 Preferences library for the simulation + +#include +#include +#include "util.h" + +class Preferences { +private: + std::map 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.h index 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.h index 088e6d4..8c8ba84 100644 --- a/gazebo/joystick.h +++ b/gazebo/joystick.h @@ -7,10 +7,6 @@ #include #include -// 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.cpp index 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.h new file mode 100644 index 0000000..404b408 --- /dev/null +++ b/gazebo/util.h @@ -0,0 +1,9 @@ +#include + +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); +}