S-01040 Motion control, step 2: 1. implemented a wrapper/frame around the heading control; 2. improved vertical motion control wrapper; 3. fixed a pwm bug related to wrong channel names being returned for some channel IDs; 4. added additional heading-related desired data parameters (drift); 5. updated main with the simple heading thread test; 6. other minor improvements and code cleanup.