AP_MotorsHeli: Create Delta Phase Angle variable and method to set. Will be used by CCComp code.

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -470,14 +470,14 @@ void AP_MotorsHeli::calculate_roll_pitch_collective_factors()
     if (_swash_type == AP_MOTORS_HELI_SWASH_CCPM) {                     //CCPM Swashplate, perform control mixing
 
         // roll factors
-        _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
-        _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
-        _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
+        _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle)));
+        _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle)));
+        _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle)));
 
         // pitch factors
-        _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
-        _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
-        _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
+        _pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle)));
+        _pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle)));
+        _pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle)));
 
         // collective factors
         _collectiveFactor[CH_1] = 1;

libraries/AP_Motors/AP_MotorsHeli.h

@@ -116,7 +116,8 @@ public:
         _rsc_runup_increment(0.0f),
         _rotor_speed_estimate(0.0f),
         _tail_direct_drive_out(0),
-        _dt(0.01f)
+        _dt(0.01f),
+        _delta_phase_angle(0)
     {
 		AP_Param::setup_object_defaults(this, var_info);
 
@@ -191,8 +192,11 @@ public:
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
     
-    // 
+    // set_dt for setting main loop rate time
     void set_dt(float dt) { _dt = dt; }
+    
+    // set_dt for setting main loop rate time
+    void set_delta_phase_angle(int16_t angle) { _delta_phase_angle = angle; }
 
 protected:
 
@@ -288,6 +292,7 @@ private:
     float           _rotor_speed_estimate;      // estimated speed of the main rotor (0~1000)
     int16_t         _tail_direct_drive_out;     // current ramped speed of output on ch7 when using direct drive variable pitch tail type
     float           _dt;                        // main loop time
+    int16_t         _delta_phase_angle;         // phase angle dynamic compensation
 };
 
 #endif  // AP_MOTORSHELI