Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Implement RC calibration, common for the real drone and the simulation

Oleg Kalachevf520b57abeaf
diff --git a/docs/build.md b/docs/build.md
index 6888f52..e25f8ed 100644
--- a/docs/build.md
+++ b/docs/build.md
@@ -91,3 +91,9 @@ Dependencies are [Gazebo Classic simulator](https://classic.gazebosim.org) and [
```
See other available Make commands in the [Makefile](../Makefile).
+
+## Setup
+
+Before flight in simulation and on the real drone, you need to calibrate your remote control. Use drone's command line interface (`make monitor` on the real drone) and type `cr` command. Copy calibration results to the source code (`flix/rc.ino` and/or `gazebo/joystick.h`).
+
+On the real drone, you also need to calibrate the accelerometer and the gyroscope. Use `ca` and `cg` commands for that. Copy calibration results to the source code (`flix/imu.ino`).
diff --git a/flix/cli.ino b/flix/cli.ino
index 2fe3465..3dd7d9e 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -27,6 +27,7 @@ const char* motd =
"rc - show RC data\n"
"mot - show motor data\n"
"log - dump in-RAM log\n"
+"cr - calibrate RC\n"
"cg - calibrate gyro\n"
"ca - calibrate accel\n"
"fullmot <n> - test motor on all signals\n"
@@ -84,6 +85,8 @@ void doCommand(String& command, String& value) {
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
} else if (command == "log") {
dumpLog();
+ } else if (command == "cr") {
+ calibrateRC();
} else if (command == "cg") {
calibrateGyro();
} else if (command == "ca") {
diff --git a/flix/rc.ino b/flix/rc.ino
index 536ecb0..cf8fb4a 100644
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -5,9 +5,9 @@
#include <SBUS.h>
-// NOTE: this should be changed to the actual calibration values
-const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
-const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
+// NOTE: use `cr` command and replace with the actual values
+int channelNeutral[] = {995, 883, 200, 972, 512, 512};
+int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
SBUS RC(Serial2);
@@ -25,8 +25,31 @@ void readRC() {
}
}
-static void normalizeRC() {
+void normalizeRC() {
for (uint8_t i = 0; i < RC_CHANNELS; i++) {
controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
}
}
+
+void calibrateRC() {
+ Serial.println("Calibrate RC: move all sticks to maximum positions within 4 seconds");
+ Serial.println("··o ··o\n··· ···\n··· ···");
+ delay(4000);
+ for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
+ for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+ channelMax[i] = channels[i];
+ }
+ Serial.println("Calibrate RC: move all sticks to neutral positions within 4 seconds");
+ Serial.println("··· ···\n··· ·o·\n·o· ···");
+ delay(4000);
+ for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated
+ for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+ channelNeutral[i] = channels[i];
+ }
+ printRCCal();
+}
+
+void printRCCal() {
+ printArray(channelNeutral, RC_CHANNELS);
+ printArray(channelMax, RC_CHANNELS);
+}
diff --git a/flix/util.ino b/flix/util.ino
index 7d145ae..f25c40f 100644
--- a/flix/util.ino
+++ b/flix/util.ino
@@ -31,3 +31,13 @@ float wrapAngle(float angle) {
}
return angle;
}
+
+template <typename T>
+void printArray(T arr[], int size) {
+ Serial.print("{");
+ for (uint8_t i = 0; i < size; i++) {
+ Serial.print(arr[i]);
+ if (i < size - 1) Serial.print(", ");
+ }
+ Serial.println("}");
+}
diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h
index f17b8f9..a2300f5 100644
--- a/gazebo/Arduino.h
+++ b/gazebo/Arduino.h
@@ -52,6 +52,10 @@ public:
return result;
}
+ size_t print(int n) {
+ return printf("%d", n);
+ }
+
size_t print(float n, int digits = 2) {
return printf("%.*f", digits, n);
}
diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
new file mode 100644
index 0000000..cd5eb3c
--- /dev/null
+++ b/gazebo/SBUS.h
@@ -0,0 +1,13 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// Repository: https://github.com/okalachev/flix
+
+// SBUS library mock to make it possible to compile simulator with rc.ino
+
+#include "joystick.h"
+
+class SBUS {
+public:
+ SBUS(HardwareSerial& bus) {};
+ void begin() {};
+ bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { joystickGet(); return true; }; // NOTE: on the hardware channels is uint16_t
+};
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 3070dbd..38540d3 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -39,6 +39,7 @@ void controlTorque();
void showTable();
bool motorsActive();
void cliTestMotor(uint8_t n);
+void printRCCal();
// mocks
void setLED(bool on) {};
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 5ee5918..84288a2 100644
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -7,10 +7,9 @@
#include <gazebo/gazebo.hh>
#include <iostream>
-// NOTE: this should be changed to the actual calibration values
-const int16_t channelNeutralMin[] = {-1290, -258, -26833, 0, 0, 0};
-const int16_t channelNeutralMax[] = {-1032, -258, -27348, 3353, 0, 0};
-const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
+// simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
+const int channelNeutralOverride[] = {-258, -258, -27349, 0, 0, 1};
+const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 0, 1};
#define RC_CHANNEL_ROLL 0
#define RC_CHANNEL_PITCH 1
@@ -34,6 +33,12 @@ 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));
}
void joystickGet() {
@@ -52,14 +57,3 @@ void joystickGet() {
normalizeRC();
}
-
-void normalizeRC() {
- for (uint8_t i = 0; i < 4; i++) {
- if (channels[i] >= channelNeutralMin[i] && channels[i] <= channelNeutralMax[i]) {
- controls[i] = 0;
- } else {
- controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
- }
- }
- controls[RC_CHANNEL_THROTTLE] = constrain(controls[RC_CHANNEL_THROTTLE], 0, 1);
-}
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index ca996bb..e624ff9 100644
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -19,7 +19,7 @@
#include "flix.h"
#include "util.h"
#include "util.ino"
-#include "joystick.h"
+#include "rc.ino"
#include "time.ino"
#include "estimate.ino"
#include "control.ino"