Port changes from commit 52819e4

docs/firmware.md

@@ -12,7 +12,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[])* — user control inputs from the RC, normalized to [-1, 1] range.
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
 * `motors` *(float[])* — motor outputs, normalized to [0, 1] range; reverse rotation is possible.
 
 ## Source files

flix/cli.ino

@@ -8,6 +8,7 @@
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float loopRate;
+extern uint16_t channels[16];
 
 const char* motd =
 "\nWelcome to\n"
@@ -46,13 +47,13 @@ void doCommand(const String& command) {
 		printIMUCal();
 		Serial.printf("rate: %f\n", loopRate);
 	} else if (command == "rc") {
-		Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
+		Serial.printf("channels: ");
-			channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],
+		for (int i = 0; i < 16; i++) {
-			channels[RC_CHANNEL_ROLL], channels[RC_CHANNEL_ARMED], channels[RC_CHANNEL_MODE]);
+			Serial.printf("%u ", channels[i]);
-		Serial.printf("Control: throttle %f yaw %f pitch %f roll %f armed %f mode %f\n",
+		}
-			controls[RC_CHANNEL_THROTTLE], controls[RC_CHANNEL_YAW], controls[RC_CHANNEL_PITCH],
+		Serial.printf("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
-			controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_ARMED], controls[RC_CHANNEL_MODE]);
+			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
-		Serial.printf("Mode: %s\n", getModeName());
+		Serial.printf("mode: %s\n", getModeName());
 	} else if (command == "mot") {
 		Serial.printf("MOTOR front-right %f front-left %f rear-right %f rear-left %f\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);

flix/control.ino

@@ -69,38 +69,39 @@ void control() {
 }
 
 void interpretRC() {
-	armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;
+	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
+
 
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controls[RC_CHANNEL_MODE] < 0.25) {
+	if (controlMode < 0.25) {
 		mode = STAB;
-	} else if (controls[RC_CHANNEL_MODE] < 0.75) {
+	} else if (controlMode < 0.75) {
 		mode = STAB;
 	} else {
 		mode = STAB;
 	}
 
-	thrustTarget = controls[RC_CHANNEL_THROTTLE];
+	thrustTarget = controlThrottle;
 
 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
-		ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
+		ratesTarget.x = controlRoll * ROLLRATE_MAX;
-		ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX;
+		ratesTarget.y = controlPitch * PITCHRATE_MAX;
-		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == STAB) {
-		yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
+		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
 
 		attitudeTarget = Quaternion::fromEuler(Vector(
-			controls[RC_CHANNEL_ROLL] * TILT_MAX,
+			controlRoll * TILT_MAX,
-			controls[RC_CHANNEL_PITCH] * TILT_MAX,
+			controlPitch * TILT_MAX,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+		ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == MANUAL) {
 		// passthrough mode
 		yawMode = YAW_RATE;
-		torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 
 	if (yawMode == YAW_RATE || !motorsActive()) {

flix/failsafe.ino

@@ -6,11 +6,11 @@
 #define RC_LOSS_TIMEOUT 0.2
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
 
-extern float controlsTime;
+extern float controlTime;
 
 // RC loss failsafe
 void failsafe() {
-	if (t - controlsTime > RC_LOSS_TIMEOUT) {
+	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
@@ -18,9 +18,9 @@ void failsafe() {
 // Smooth descend on RC lost
 void descend() {
 	mode = STAB;
-	controls[RC_CHANNEL_ROLL] = 0;
+	controlRoll = 0;
-	controls[RC_CHANNEL_PITCH] = 0;
+	controlPitch = 0;
-	controls[RC_CHANNEL_YAW] = 0;
+	controlYaw = 0;
-	controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME;
+	controlThrottle -= dt / DESCEND_TIME;
-	if (controls[RC_CHANNEL_THROTTLE] < 0) controls[RC_CHANNEL_THROTTLE] = 0;
+	if (controlThrottle < 0) controlThrottle = 0;
 }

flix/flix.ino

@@ -10,17 +10,9 @@
 #define SERIAL_BAUDRATE 115200
 #define WIFI_ENABLED 1
 
-#define RC_CHANNEL_ROLL 0
-#define RC_CHANNEL_PITCH 1
-#define RC_CHANNEL_THROTTLE 2
-#define RC_CHANNEL_YAW 3
-#define RC_CHANNEL_ARMED 4
-#define RC_CHANNEL_MODE 5
-
 float 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

@@ -13,7 +13,7 @@
 #define MAVLINK_CONTROL_SCALE 0.7f
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
-extern float controlsTime;
+extern float controlTime;
 
 void processMavlink() {
 	sendMavlink();
@@ -45,10 +45,8 @@ 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,
-			INT16_MAX, INT16_MAX, UINT8_MAX);
 		sendMessage(&msg);
 
 		float actuator[32];
@@ -90,15 +88,15 @@ void handleMavlink(const void *_msg) {
 	if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
 		mavlink_manual_control_t m;
 		mavlink_msg_manual_control_decode(&msg, &m);
-		controls[RC_CHANNEL_THROTTLE] = m.z / 1000.0f;
+		controlThrottle = m.z / 1000.0f;
-		controls[RC_CHANNEL_PITCH] = m.x / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controlPitch = m.x / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controls[RC_CHANNEL_ROLL] = m.y / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controlRoll = m.y / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controls[RC_CHANNEL_YAW] = m.r / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controlYaw = m.r / 1000.0f * MAVLINK_CONTROL_SCALE;
-		controls[RC_CHANNEL_MODE] = 1; // STAB mode
+		controlMode = 1; // STAB mode
-		controls[RC_CHANNEL_ARMED] = 1; // armed
+		controlArmed = 1; // armed
-		controlsTime = t;
+		controlTime = t;
 
-		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
+		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
 	}
 }
 

flix/rc.ino

@@ -8,10 +8,21 @@
 
 SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
 
+uint16_t channels[16]; // raw rc channels
+float controlTime; // time of the last controls update
+
+
 // NOTE: use 'cr' command to calibrate the RC and put the values here
-int channelNeutral[] = {995, 883, 200, 972, 512, 512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int channelZero[] = {992, 992, 172, 992, 172, 172, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int channelMax[] = {1811, 1811, 1811, 1811, 1811, 1811, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-float controlsTime; // time of the last controls update
+
+// Channels mapping:
+int rollChannel = 0;
+int pitchChannel = 1;
+int throttleChannel = 2;
+int yawChannel = 3;
+int armedChannel = 4;
+int modeChannel = 5;
 
 void setupRC() {
 	Serial.println("Setup RC");
@@ -21,18 +32,26 @@ 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() {
-	for (uint8_t i = 0; i < 16; i++) {
+	float controls[16];
-		controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
+	for (int i = 0; i < 16; i++) {
+		controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
 	}
+	// Update control values
+	controlRoll = controls[rollChannel];
+	controlPitch = controls[pitchChannel];
+	controlYaw = controls[yawChannel];
+	controlThrottle = controls[throttleChannel];
+	controlArmed = controls[armedChannel];
+	controlMode = controls[modeChannel];
 }
 
 void calibrateRC() {
@@ -48,13 +67,13 @@ void calibrateRC() {
 	delay(4000);
 	while (!readRC());
 	for (int i = 0; i < 16; i++) {
-		channelNeutral[i] = channels[i];
+		channelZero[i] = channels[i];
 	}
-	printRCCal();
+	printRCCalibration();
 }
 
-void printRCCal() {
+void printRCCalibration() {
-	for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) Serial.printf("%d ", channelNeutral[i]);
+	for (int i = 0; i < sizeof(channelZero) / sizeof(channelZero[0]); i++) Serial.printf("%d ", channelZero[i]);
 	Serial.printf("\n");
 	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%d ", channelMax[i]);
 	Serial.printf("\n");

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 @@
 float 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;
@@ -36,7 +35,7 @@ void sendMotors();
 bool motorsActive();
 void doCommand(const String& command);
 void normalizeRC();
-void printRCCal();
+void printRCCalibration();
 void processMavlink();
 void sendMavlink();
 void sendMessage(const void *msg);

gazebo/joystick.h

@@ -8,15 +8,16 @@
 #include <iostream>
 
 // simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
+const int channelZeroOverride[] = {1500, 0, 1000, 1500, 1500, 1000};
-const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
+const int channelMaxOverride[] = {2000, 2000, 2000, 2000, 2000, 2000};
 
-#define RC_CHANNEL_ROLL 0
+// channels mapping overrides
-#define RC_CHANNEL_PITCH 1
+const int rollChannelOverride = 3;
-#define RC_CHANNEL_THROTTLE 2
+const int pitchChannelOverride = 4;
-#define RC_CHANNEL_YAW 3
+const int throttleChannelOverride = 5;
-#define RC_CHANNEL_ARMED 5
+const int yawChannelOverride = 0;
-#define RC_CHANNEL_MODE 4
+const int armedChannelOverride = 2;
+const int modeChannelOverride = 1;
 
 SDL_Joystick *joystick;
 
@@ -37,12 +38,20 @@ bool joystickInit() {
 		warnShown = true;
 	}
 
-	// apply calibration overrides
+	// apply overrides
-	extern int channelNeutral[16];
+	extern int channelZero[16];
 	extern int channelMax[16];
-	memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
+	memcpy(channelZero, channelZeroOverride, sizeof(channelZeroOverride));
 	memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
 
+	extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+	rollChannel = rollChannelOverride;
+	pitchChannel = pitchChannelOverride;
+	throttleChannel = throttleChannelOverride;
+	yawChannel = yawChannelOverride;
+	armedChannel = armedChannelOverride;
+	modeChannel = modeChannelOverride;
+
 	return joystickInitialized;
 }
 

gazebo/simulator.cpp

@@ -70,8 +70,8 @@ public:
 
 		// read rc
 		readRC();
-		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
+		controlMode = 1; // 0 acro, 1 stab
-		controls[RC_CHANNEL_ARMED] = 1; // armed
+		controlArmed = 1; // armed
 
 		estimate();