The nav_pitch_cd filtering has been added.
To explain the concept a bit further, the idea is that rather than try to run copter control loops using a body frame of reference, we instead use a rotor frame of reference which is a combination of the angle of the body plus the tilt angle of the rotors relative to the body averaged across the rotors.
The 'Twin Vectored Belly Sitter' has been added as separate frame class and is selected by setting Q_FRAME_CLASS = 13
We then use the nav_pitch demand to specify a desired rotor orientation in earth frame. This demand is rate limited according to the Q_TVBS_SLEW_LIM parameter with default value of 60 deg/sec. The rate limited pitch demand is also filtered using first order low-pass filter as specified by the Q_TVBS_SLEW_TAU time constant wiht a default value of 0.2 sec. This filtering ensures that the tilt mechanism will not be asked to to rapid angle changes that is cannot follow.
The tilt servos are adjusted to keep the rotors pointing in the direction specified by the nav_pitch_cd variable. The mapping from demanded motor tilt to servo deflection is specified by the Q_TVBS_ANGMIN and Q_TVBS_ANGMAX parameters and the servo PWM max, min and trim parameters for the tilt servos.
In effect we are pointing the motors similar to how a servo camera gimbal is pointed.The wing is allowed to assume it's preferred angle underneath the rotors. The elevator is used to damp the body pitch motion so the motor pointing control does not have to compensate for large body rates . This aerodynamic control damping is controlled by the Q_TVBS_QGAIN parameter. Some additional pitch damping is also fed into the tilt motors and controlled by the Q_TVBS_TILT_LAG parameter.
This means the copter control laws are demanding the angle of a rotor disc which is what they are designed to do.
Control of yaw and roll has been left alone for the time being, however I will be adding a gain parameter so that we can also adjust the amount of fraction of aileron control action sent tot e control surfaces during VTOL operation.