mavstation-firmware refactored more