Added transform to right CS
Cleaned up
Minor changes due to higher IMU pub frequency
Added code to find and update desired plane
Correct serial message to include set command
Everything compiles
i'm done
Corrected plane finding algorithm
Final commit