Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Fix attitude error calculation in sim
Oleg Kalachev02aac609ab6a
diff --git a/gazebo/flix.cpp b/gazebo/flix.cppindex 0fb6177..c610704 100644--- a/gazebo/flix.cpp+++ b/gazebo/flix.cpp@@ -190,7 +190,8 @@ public:// calculate attitude estimation error- float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));+ Quaternion groundtruthAttitude(estimateModel->WorldPose().Rot().W(), estimateModel->WorldPose().Rot().X(), -estimateModel->WorldPose().Rot().Y(), -estimateModel->WorldPose().Rot().Z());+ float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), groundtruthAttitude.rotate(Vector(0, 0, -1)));if (angle < 0.3) {//gzwarn << "att err: " << angle << endl;// TODO: warning