Fixed more logging, discovered problem caused by rogue atan for angle of motion.
Fixed drag and calculation of angle of motion!