I am trying to simulate a custom BlueRov2 heavy with SITL + Gazebo (Using ardupilot Sub-3.6). I have modified the sdf file so it accepts 8 command channels corresponds to 8 motors, I am able to control the thrusters with controller, however the depth hold mode does not work properly, it seems like SITL is not getting depth and IMU feedback from simulation, the depth shown on QGC is correct for a few seconds, then when I make the robot dive deeper, the number starts to jump to -10, -20, -30 … The robot orientation is also faulty.
Then I tried to dig into the firmware.
The fdm_packet sent by ardupilot gazebo plugin are just pose, velocity and acceleration, the packet is received by Gazebo::recv_fdm, and does not seem to be used else where for depth hold.
Then I noticed the sensor data captured by QGC is from AP_Baro.get_altitude().
Am I on the right track? Or did I do something inappropriate?
Any help appreciated.