C based mavlink fixes

Fixing some errors.