Added mavlink from master
Added sonar interrupt message
Added dummy handler for sonar interrupt
more debugging