Copter: new ROLL_PITCH_WP_INAV roll-pitch mode

small corrections to allow waypoints to work with new inertial nav wp
controller