Telemetry refactoring - firmware fixes