Nav log fix

This fixes the navigation state logging to make debugging navigator simpler, and it only logs the setpoint if at least current is valid. This hopefully eleminates the weird zeros we're currently seeing in logs.