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

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`).

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") {

flix/rc.ino

@@ -5,9 +5,9 @@
 
 #include <SBUS.h>
 
-// NOTE: this should be changed to the actual calibration values
+// NOTE: use `cr` command and replace with the actual values
-const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
+int channelNeutral[] = {995, 883, 200, 972, 512, 512};
-const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
+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);
+}

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("}");
+}

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);
 	}

gazebo/SBUS.h

@@ -0,0 +1,13 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// 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
+};

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) {};

gazebo/joystick.h

@@ -7,10 +7,9 @@
 #include <gazebo/gazebo.hh>
 #include <iostream>
 
-// NOTE: this should be changed to the actual calibration values
+// simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int16_t channelNeutralMin[] = {-1290, -258, -26833, 0, 0, 0};
+const int channelNeutralOverride[] = {-258, -258, -27349, 0, 0, 1};
-const int16_t channelNeutralMax[] = {-1032, -258, -27348, 3353, 0, 0};
+const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 0, 1};
-const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
 
 #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);
-}

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"