Added a workmode for selecting a plane (not finished) Set workmode global var to 0 for normal operation or to 3 for plane finding mode