Rewrite of Martinez brushless gimbal firmware with various improvements