This is branch for KMTI Ardupilot modified FW