Use FLU as the main coordinate system instead of FRD

Corresponding to the IMU orientation in the new version

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
-		gyro = flu2frd(imu->AngularVelocity());
-		acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
+		// read virtual imu
+		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();
@@ -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());
-}