Major rework of rc subsystem

Implement channels mapping calibration.
Store mapping in parameters.
Get rid of `controls` array and store control inputs in `controlRoll`, `controlPitch`, ... variables.
Move `channels` variable to rc.ino, channels are not involved when controled using mavlink.
'Neutral' values are renamed to 'zero' - more precise naming.
`controlsTime` is renamed to `controlTime`.
Use unsigned values for channels.
Make channel values in simulation more alike to real world: unsigned values in range [1000, 2000].
Send RC_CHANNELS_RAW instead of RC_CHANNELS_SCALED via mavlink
Don't send channels data via mavlink if rc is not used

docs/book/firmware.md

@@ -10,7 +10,7 @@
 * `acc` *(Vector)* — данные с акселерометра, *м/с<sup>2</sup>*.
 * `rates` *(Vector)* — отфильтрованные угловые скорости, *рад/с*.
 * `attitude` *(Quaternion)* — оценка ориентации (положения) дрона.
-* `controls` *(float[])* — команды управления от пилота, в диапазоне [-1, 1].
+* `controlRoll`, `controlPitch`, ... *(float[])* — команды управления от пилота, в диапазоне [-1, 1].
 * `motors` *(float[])* — выходные сигналы на моторы, в диапазоне [0, 1].
 
 ## Исходные файлы

docs/firmware.md

@@ -14,7 +14,7 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa
 * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.
 * `rates` *(Vector)* — filtered angular rates, *rad/s*.
 * `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
-* `controls` *(float[])* — pilot's control inputs, range [-1, 1].
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
 * `motors` *(float[])* — motor outputs, range [0, 1].
 
 ## Source files

flix/cli.ino

@@ -10,7 +10,8 @@
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float loopRate, dt;
 extern double t;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern uint16_t channels[16];
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 const char* motd =
 "\nWelcome to\n"
@@ -109,13 +110,13 @@ void doCommand(String str, bool echo = false) {
 		print("rate: %.0f\n", loopRate);
 		print("landed: %d\n", landed);
 	} else if (command == "rc") {
-		print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
+		print("channels: ");
-			channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
+		for (int i = 0; i < 16; i++) {
-			channels[rollChannel], channels[armedChannel], channels[modeChannel]);
+			print("%u ", channels[i]);
-		print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
+		}
-			controls[throttleChannel], controls[yawChannel], controls[pitchChannel],
+		print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
-			controls[rollChannel], controls[armedChannel], controls[modeChannel]);
+			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
-		print("Mode: %s\n", getModeName());
+		print("mode: %s\n", getModeName());
 	} else if (command == "mot") {
 		print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);

flix/control.ino

@@ -21,7 +21,7 @@
 #define YAWRATE_I 0.0
 #define YAWRATE_D 0.0
 #define YAWRATE_I_LIM 0.3
-#define ROLL_P 4.5
+#define ROLL_P 6
 #define ROLL_I 0
 #define ROLL_D 0
 #define PITCH_P ROLL_P
@@ -54,7 +54,7 @@ Vector torqueTarget;
 float thrustTarget;
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 void control() {
 	interpretRC();
@@ -72,39 +72,38 @@ void control() {
 }
 
 void interpretRC() {
-	armed = controls[throttleChannel] >= 0.05 &&
+	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
-		(controls[armedChannel] >= 0.5 || isnan(controls[armedChannel])); // assume armed if armed channel is not defined
 
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controls[modeChannel] < 0.25) {
+	if (controlMode < 0.25) {
 		mode = STAB;
-	} else if (controls[modeChannel] < 0.75) {
+	} else if (controlMode < 0.75) {
 		mode = STAB;
 	} else {
 		mode = STAB;
 	}
 
-	thrustTarget = controls[throttleChannel];
+	thrustTarget = controlThrottle;
 
 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
-		ratesTarget.x = controls[rollChannel] * maxRate.x;
+		ratesTarget.x = controlRoll * maxRate.x;
-		ratesTarget.y = controls[pitchChannel] * maxRate.y;
+		ratesTarget.y = controlPitch* maxRate.y;
-		ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == STAB) {
-		yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE;
+		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
 
 		attitudeTarget = Quaternion::fromEuler(Vector(
-			controls[rollChannel] * tiltMax,
+			controlRoll * tiltMax,
-			controls[pitchChannel] * tiltMax,
+			controlPitch * tiltMax,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == MANUAL) {
 		// passthrough mode
 		yawMode = YAW_RATE;
-		torqueTarget = Vector(controls[rollChannel], controls[pitchChannel], -controls[yawChannel]) * 0.01;
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 
 	if (yawMode == YAW_RATE || !motorsActive()) {

flix/failsafe.ino

@@ -6,8 +6,8 @@
 #define RC_LOSS_TIMEOUT 0.2
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
 
-extern double controlsTime;
+extern double controlTime;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 void failsafe() {
 	armingFailsafe();
@@ -19,13 +19,13 @@ void armingFailsafe() {
 	static double zeroThrottleTime;
 	static double armingTime;
 	if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
-	if (controlsTime > 0 && controls[throttleChannel] < 0.05) zeroThrottleTime = controlsTime;
+	if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
 	if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
 }
 
 // RC loss failsafe
 void rcLossFailsafe() {
-	if (t - controlsTime > RC_LOSS_TIMEOUT) {
+	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
@@ -33,9 +33,9 @@ void rcLossFailsafe() {
 // Smooth descend on RC lost
 void descend() {
 	mode = STAB;
-	controls[rollChannel] = 0;
+	controlRoll = 0;
-	controls[pitchChannel] = 0;
+	controlPitch = 0;
-	controls[yawChannel] = 0;
+	controlYaw = 0;
-	controls[throttleChannel] -= dt / DESCEND_TIME;
+	controlThrottle -= dt / DESCEND_TIME;
-	if (controls[throttleChannel] < 0) controls[throttleChannel] = 0;
+	if (controlThrottle < 0) controlThrottle = 0;
 }

flix/flix.ino

@@ -12,8 +12,7 @@
 
 double t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-int16_t channels[16]; // raw rc channels
+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
-float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s

flix/mavlink.ino

@@ -15,8 +15,8 @@
 float mavlinkControlScale = 0.7;
 String mavlinkPrintBuffer;
 
-extern double controlsTime;
+extern double controlTime;
-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 void processMavlink() {
 	sendMavlink();
@@ -54,11 +54,9 @@ void sendMavlink() {
 			time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
 		sendMessage(&msg);
 
-		mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
+		mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
-			controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
+			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
-			controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
+		if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
-			INT16_MAX, INT16_MAX, UINT8_MAX);
-		sendMessage(&msg);
 
 		float actuator[32];
 		memcpy(actuator, motors, sizeof(motors));
@@ -101,15 +99,15 @@ void handleMavlink(const void *_msg) {
 		mavlink_msg_manual_control_decode(&msg, &m);
 		if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
 
-		controls[throttleChannel] = m.z / 1000.0f;
+		controlThrottle = m.z / 1000.0f;
-		controls[pitchChannel] = m.x / 1000.0f * mavlinkControlScale;
+		controlPitch = m.x / 1000.0f * mavlinkControlScale;
-		controls[rollChannel] = m.y / 1000.0f * mavlinkControlScale;
+		controlRoll = m.y / 1000.0f * mavlinkControlScale;
-		controls[yawChannel] = m.r / 1000.0f * mavlinkControlScale;
+		controlYaw = m.r / 1000.0f * mavlinkControlScale;
-		controls[modeChannel] = 1; // STAB mode
+		controlMode = 1; // STAB mode
-		controls[armedChannel] = 1; // armed
+		controlArmed = 1; // armed
-		controlsTime = t;
+		controlTime = t;
 
-		if (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;
+		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
 	}
 
 	if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {

flix/parameters.ino

@@ -5,8 +5,9 @@
 
 #include <Preferences.h>
 
-extern float channelNeutral[16];
+extern float channelZero[16];
 extern float channelMax[16];
+extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern float mavlinkControlScale;
 
 Preferences storage;
@@ -49,14 +50,14 @@ Parameter parameters[] = {
 	{"ACC_SCALE_Y", &accScale.y},
 	{"ACC_SCALE_Z", &accScale.z},
 	// rc
-	{"RC_NEUTRAL_0", &channelNeutral[0]},
+	{"RC_ZERO_0", &channelZero[0]},
-	{"RC_NEUTRAL_1", &channelNeutral[1]},
+	{"RC_ZERO_1", &channelZero[1]},
-	{"RC_NEUTRAL_2", &channelNeutral[2]},
+	{"RC_ZERO_2", &channelZero[2]},
-	{"RC_NEUTRAL_3", &channelNeutral[3]},
+	{"RC_ZERO_3", &channelZero[3]},
-	{"RC_NEUTRAL_4", &channelNeutral[4]},
+	{"RC_ZERO_4", &channelZero[4]},
-	{"RC_NEUTRAL_5", &channelNeutral[5]},
+	{"RC_ZERO_5", &channelZero[5]},
-	{"RC_NEUTRAL_6", &channelNeutral[6]},
+	{"RC_ZERO_6", &channelZero[6]},
-	{"RC_NEUTRAL_7", &channelNeutral[7]},
+	{"RC_ZERO_7", &channelZero[7]},
 	{"RC_MAX_0", &channelMax[0]},
 	{"RC_MAX_1", &channelMax[1]},
 	{"RC_MAX_2", &channelMax[2]},
@@ -65,6 +66,12 @@ Parameter parameters[] = {
 	{"RC_MAX_5", &channelMax[5]},
 	{"RC_MAX_6", &channelMax[6]},
 	{"RC_MAX_7", &channelMax[7]},
+	{"RC_ROLL", &rollChannel},
+	{"RC_PITCH", &pitchChannel},
+	{"RC_THROTTLE", &throttleChannel},
+	{"RC_YAW", &yawChannel},
+	{"RC_ARMED", &armedChannel},
+	{"RC_MODE", &modeChannel},
 #if WIFI_ENABLED
 	// MAVLink
 	{"MAV_CTRL_SCALE", &mavlinkControlScale},

flix/rc.ino

@@ -8,17 +8,14 @@
 
 SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
-// RC channels mapping:
+uint16_t channels[16]; // raw rc channels
-int rollChannel = 0;
+double controlTime; // time of the last controls update
-int pitchChannel = 1;
+float channelZero[16]; // calibration zero values
-int throttleChannel = 2;
+float channelMax[16]; // calibration max values
-int yawChannel = 3;
+
-int armedChannel = 4;
+// Channels mapping (using float to store in parameters):
-int modeChannel = 5;
+float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, armedChannel = NAN, modeChannel = NAN;
 
-double controlsTime; // time of the last controls update
-float channelNeutral[16] = {NAN}; // first element NAN means not calibrated
-float channelMax[16];
 
 void setupRC() {
 	print("Setup RC\n");
@@ -28,42 +25,76 @@ void setupRC() {
 bool readRC() {
 	if (RC.read()) {
 		SBUSData data = RC.data();
-		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
+		for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
 		normalizeRC();
-		controlsTime = t;
+		controlTime = t;
 		return true;
 	}
 	return false;
 }
 
 void normalizeRC() {
-	if (isnan(channelNeutral[0])) return; // skip if not calibrated
+	float controls[16];
-	for (uint8_t i = 0; i < 16; i++) {
+	for (int i = 0; i < 16; i++) {
-		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
+		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
 	}
+	// Update control values
+	controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;
+	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
+	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
+	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
+	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
+	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 
 void calibrateRC() {
-	print("Calibrate RC: move all sticks to maximum positions [4 sec]\n");
+	uint16_t zero[16];
-	print("··o     ··o\n···     ···\n···     ···\n");
+	uint16_t center[16];
-	pause(4);
+	uint16_t max[16];
-	while (!readRC());
+	print("1/9 Calibrating RC: put all switches to default positions [3 sec]\n");
-	for (int i = 0; i < 16; i++) {
+	pause(3);
-		channelMax[i] = channels[i];
+	calibrateRCChannel(NULL, zero, zero, "2/9 Move sticks [3 sec]\n...     ...\n...     .o.\n.o.     ...\n");
-	}
+	calibrateRCChannel(NULL, center, center, "3/9 Move sticks [3 sec]\n...     ...\n.o.     .o.\n...     ...\n");
-	print("Calibrate RC: move all sticks to neutral positions [4 sec]\n");
+	calibrateRCChannel(&throttleChannel, zero, max, "4/9 Move sticks [3 sec]\n.o.     ...\n...     .o.\n...     ...\n");
-	print("···     ···\n···     ·o·\n·o·     ···\n");
+	calibrateRCChannel(&yawChannel, center, max, "5/9 Move sticks [3 sec]\n...     ...\n..o     .o.\n...     ...\n");
-	pause(4);
+	calibrateRCChannel(&pitchChannel, zero, max, "6/9 Move sticks [3 sec]\n...     .o.\n...     ...\n.o.     ...\n");
-	while (!readRC());
+	calibrateRCChannel(&rollChannel, zero, max, "7/9 Move sticks [3 sec]\n...     ...\n...     ..o\n.o.     ...\n");
-	for (int i = 0; i < 16; i++) {
+	calibrateRCChannel(&armedChannel, zero, max, "8/9 Switch to armed [3 sec]\n");
-		channelNeutral[i] = channels[i];
+	calibrateRCChannel(&modeChannel, zero, max, "9/9 Disarm and switch mode to max [3 sec]\n");
-	}
+	printRCCalibration();
-	printRCCal();
 }
 
-void printRCCal() {
+void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
-	for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);
+	print("%s", str);
-	print("\n");
+	pause(3);
-	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);
+	for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
-	print("\n");
+	memcpy(out, channels, sizeof(channels));
+
+	if (channel == NULL) return; // no channel to calibrate
+
+	// Find channel that changed the most between in and out
+	int ch = -1, diff = 0;
+	for (int i = 0; i < 16; i++) {
+		if (abs(out[i] - in[i]) > diff) {
+			ch = i;
+			diff = abs(out[i] - in[i]);
+		}
+	}
+	if (ch >= 0 && diff > 10) { // difference threshold is 10
+		*channel = ch;
+		channelZero[ch] = in[ch];
+		channelMax[ch] = out[ch];
+	} else {
+		*channel = NAN;
+	}
+}
+
+void printRCCalibration() {
+	print("Control   Ch     Zero   Max\n");
+	print("Roll      %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
+	print("Pitch     %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
+	print("Yaw       %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
+	print("Throttle  %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
+	print("Armed     %-7g%-7g%-7g\n", armedChannel, armedChannel >= 0 ? channelZero[(int)armedChannel] : NAN, armedChannel >= 0 ? channelMax[(int)armedChannel] : NAN);
+	print("Mode      %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
 }

gazebo/SBUS.h

@@ -18,6 +18,9 @@ public:
 	SBUSData data() {
 		SBUSData data;
 		joystickGet(data.ch);
+		for (int i = 0; i < 16; i++) {
+			data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
+		}
 		return data;
 	};
 };

gazebo/flix.h

@@ -15,8 +15,7 @@
 double t = NAN;
 float dt;
 float motors[4];
-int16_t channels[16]; // raw rc channels
+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
-float controls[16];
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -41,9 +40,10 @@ void print(const char* format, ...);
 void pause(float duration);
 void doCommand(String str, bool echo);
 void handleInput();
-void calibrateRC();
 void normalizeRC();
-void printRCCal();
+void calibrateRC();
+void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
+void printRCCalibration();
 void dumpLog();
 void processMavlink();
 void sendMavlink();

gazebo/simulator.cpp

@@ -71,11 +71,7 @@ public:
 		gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
 		acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
 
-		// read rc
 		readRC();
-		controls[modeChannel] = 1; // 0 acro, 1 stab
-		controls[armedChannel] = 1; // armed
-
 		estimate();
 
 		// correct yaw to the actual yaw