Basic Tricopter Control Code - In Progress