I had two crashes during GPS-denied indoor flights and I think I found the cause, but I am not a MAVROS expert so I would love to hear if anyone else has seen this.
We are running a ZED 2i into ArduPilot EKF3 via MAVROS on ROS Humble / Ubuntu 22.04, publishing vision pose to /mavros/vision_pose/pose. Two things kept happening:
First symptom: every time the drone rotated past roughly 180°, the yaw reported to ArduPilot would fold back instead of continuing. A full 360° spin produced 0 → π → 0 → π instead of the correct 0 → π → −π → 0. So as the drone kept rotating, EKF3 was seeing the heading suddenly reverse direction — and would try to correct for it.
Second symptom: sometimes the drone would start up with a yaw of ~3.14 rad. Since that is so close to 0 in the EKF’s view of the world, it would arm fine — but then fly toward the operator when commanded to go the other way, because its sense of north was flipped.
After a lot of head-scratching I traced it to this function inside MAVROS (mavros/src/lib/ftf_quaternion_utils.cpp):
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q)
{
// YPR - ZYX
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
From what I can tell, Eigen::eulerAngles(2,1,0) always returns the primary axis (yaw, in ZYX) in [0, π]. Any heading past 180° gets folded back into that range rather than going negative. I replaced it with an explicit atan2 decomposition:
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q)
{
auto m = q.toRotationMatrix();
return Eigen::Vector3d(
std::atan2(m(2, 1), m(2, 2)), // roll
std::asin(-m(2, 0)), // pitch
std::atan2(m(1, 0), m(0, 0)) // yaw — now covers (−π, π]
);
}
After that change the yaw folding stopped and we have not had another crash from this.
Looking through the MAVROS issue tracker I found this has apparently been reported before — #358 (2015), #444 (2015), and #1472 (2020) all describe the same [0, π] yaw range problem — but as far as I can tell no fix was ever merged, and the ROS2 port in 2021 carried it forward. I also read that publishing to /mavros/mocap/pose instead might avoid this entirely since that plugin sends a quaternion directly, though I have not tested it myself.
I opened a PR with the fix: mavlink/mavros#2232.
Has anyone else had this problem or found a different explanation? Would also be curious if this only affects ROS Humble or if earlier versions have it too.