Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

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.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"
-"<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.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 <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.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, &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
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 <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();
+}
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 <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);
}
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 <[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.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 <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.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 <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);
+}