Stonefish with Ardusub - Pitch not accurate - STABILIZE & ALT_HOLD not working

Basically, the AUV works fine (ish) in MANUAL mode, there’s some thruster flipping and its not as responsive as we expected but ArduSub seems to always end up with an incorrect value of pitch / roll that causes the STABLIZE mode to just rotate the bot contiuously

It seems to be stable for a little bit initially but then will just start flipping around continuously - it seems to behave slightly differently on different people’s machines also but this same pattern persists

Essentially:
it is supposed to stably maintain pitch and roll on a neutral position but it ramps up and goes out of control

The bot seems to think its upside down, We’ve tried a few things like setting various rotations of flight computer in ardusub, setting pitch to 0 from the SITL bridge script and a few other things but it seems to always return to this

Demonstration video:

WhatsApp Video 2026-02-12 at 8.46.23 PM.mp4

Repository has our stonefish simulator, the interconnect script and the ardusub SITL dockerfile & docker-compose - Ive written a few setup instructions in the wiki:

The nishaanth_version branch has some various tweaks we tried to fix it but that didn’t fully resolve it but seemed to help but we aren’t sure exactly what it was

Ive tried finding any solutions but while it seems to be a recurring issue with SITL - I havent seen anything that resolved the issue