Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Implement RC calibration, common for the real drone and the simulation
Oleg Kalachevf520b57abeaf
diff --git a/docs/build.md b/docs/build.mdindex 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.inoindex 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.inoindex 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.inoindex 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.hindex 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.hnew file mode 100644index 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.hindex 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();// mocksvoid setLED(bool on) {};diff --git a/gazebo/joystick.h b/gazebo/joystick.hindex 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.cppindex 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"