From f520b57abeaf9083e26ea88440adb98af9ae6210 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 2 Jan 2024 11:54:09 +0300 Subject: [PATCH] Implement RC calibration, common for the real drone and the simulation --- docs/build.md | 6 ++++++ flix/cli.ino | 3 +++ flix/rc.ino | 31 +++++++++++++++++++++++++++---- flix/util.ino | 10 ++++++++++ gazebo/Arduino.h | 4 ++++ gazebo/SBUS.h | 13 +++++++++++++ gazebo/flix.h | 1 + gazebo/joystick.h | 24 +++++++++--------------- gazebo/simulator.cpp | 2 +- 9 files changed, 74 insertions(+), 20 deletions(-) create mode 100644 gazebo/SBUS.h 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 - 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 -// 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 +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 +// 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 #include -// 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"