Add sanity test to the gimbal code