Fixed a stupid bug where I was using the wrong data type for the last_cmd_time
Commented out the debug message
Also realized that last_heartbeat_ms should also be assigned in set_RPY. This should allow me to re enable the GCS failsafe, like I ideally should :)