Mavlink stack usage

WIP, will do this tonight.