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

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

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