Use FLU as the main coordinate system instead of FRD

Corresponding to the IMU orientation in the new version

flix/control.ino

@@ -81,22 +81,22 @@ void interpretRC() {
 	if (mode == ACRO) {
 		yawMode = YAW_RATE;
 		ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
-		ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd
+		ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX;
-		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
+		ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
 
 	} else if (mode == STAB) {
 		yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
 
 		attitudeTarget = Quaternion::fromEulerZYX(Vector(
 			controls[RC_CHANNEL_ROLL] * MAX_TILT,
-			-controls[RC_CHANNEL_PITCH] * MAX_TILT,
+			controls[RC_CHANNEL_PITCH] * MAX_TILT,
 			attitudeTarget.getYaw()));
-		ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
+		ratesTarget.z = -controls[RC_CHANNEL_YAW] * 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(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;
 	}
 
 	if (yawMode == YAW_RATE || !motorsActive()) {
@@ -113,7 +113,7 @@ void controlAttitude() {
 		return;
 	}
 
-	const Vector up(0, 0, -1);
+	const Vector up(0, 0, 1);
 	Vector upActual = attitude.rotate(up);
 	Vector upTarget = attitudeTarget.rotate(up);
 
@@ -150,10 +150,10 @@ void controlTorque() {
 		return;
 	}
 
-	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
+	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;
-	motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
+	motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;
-	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;
+	motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
-	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;
+	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
 
 	motors[0] = constrain(motors[0], 0, 1);
 	motors[1] = constrain(motors[1], 0, 1);

flix/estimate.ino

@@ -36,7 +36,7 @@ void applyAcc() {
 	if (!landed) return;
 
 	// calculate accelerometer correction
-	Vector up = attitude.rotate(Vector(0, 0, -1));
+	Vector up = attitude.rotate(Vector(0, 0, 1));
 	Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
 
 	// apply correction
@@ -45,6 +45,6 @@ void applyAcc() {
 }
 
 void signalizeHorizontality() {
-	float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
+	float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, 1)), Vector(0, 0, 1));
 	setLED(angle < radians(15));
 }

flix/imu.ino

@@ -2,6 +2,9 @@
 // Repository: https://github.com/okalachev/flix
 
 // Work with the IMU sensor
+// IMU is oriented FLU (front-left-up) style.
+// In case of FRD (front-right-down) orientation of the IMU, use this code:
+// https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98.
 
 #include <SPI.h>
 #include <MPU9250.h>

flix/mavlink.ino

@@ -38,8 +38,9 @@ void sendMavlink() {
 		lastFast = t;
 
 		const float zeroQuat[] = {0, 0, 0, 0};
+		Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system
 		mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
-			time, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat);
+			time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
 		sendMessage(&msg);
 
 		mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0,
@@ -97,4 +98,9 @@ void handleMavlink(const void *_msg) {
 	}
 }
 
+// Convert Forward-Left-Up to Forward-Right-Down quaternion
+inline Quaternion FLU2FRD(const Quaternion &q) {
+	return Quaternion(q.w, q.x, -q.y, -q.z);
+}
+
 #endif

gazebo/flix.h

@@ -49,6 +49,7 @@ void sendMavlink();
 void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
+inline Quaternion FLU2FRD(const Quaternion &q);
 
 // mocks
 void setLED(bool on) {};

gazebo/simulator.cpp

@@ -17,7 +17,6 @@
 
 #include "Arduino.h"
 #include "flix.h"
-#include "util.h"
 #include "util.ino"
 #include "rc.ino"
 #include "time.ino"
@@ -63,9 +62,9 @@ public:
 		__micros = model->GetWorld()->SimTime().Double() * 1000000;
 		step();
 
-		// read imu
+		// read virtual imu
-		gyro = flu2frd(imu->AngularVelocity());
+		gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
-		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
+		acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
 
 		// read rc
 		readRC();
@@ -75,7 +74,7 @@ public:
 		estimate();
 
 		// correct yaw to the actual yaw
-		attitude.setYaw(-this->model->WorldPose().Yaw());
+		attitude.setYaw(this->model->WorldPose().Yaw());
 
 		control();
 		parseInput();

gazebo/util.h

@@ -1,14 +0,0 @@
-#include <ignition/math/Vector3.hh>
-#include <ignition/math/Pose3.hh>
-
-using ignition::math::Vector3d;
-using ignition::math::Pose3d;
-
-Pose3d flu2frd(const Pose3d& p) {
-	return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),
-	                              p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
-}
-
-Vector flu2frd(const Vector3d& v) {
-	return Vector(v.X(), -v.Y(), -v.Z());
-}