Teensy 3.1 code to control four LEDs with MAVLink heartbeat messages