ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTms.c