px4io_uploader cleanup and minor optimization

Remove redundant code
Cleanup error handling in program function
Tested on V1 and V2 PX4
Debug only, but we really only need to check once for multiple of 4 instead of each time we loop