Added setpoint initialization and added command rx functionality.  No cmd parse added