AP_MotorsHeli: Add Helpers for blade pitch angle logging

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -147,6 +147,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),
 
+    // @Param: CYC_MAX_ANG
+    // @DisplayName: Blade pitch contribution from cyclic output.
+    // @Description: The angle, in degrees, of the blade pitch angle contribution that the cyclic adds at maximum output.
+    // @Range: 0 10
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("CYC_MAX_ANG", 20, AP_MotorsHeli, _cyclic_angle_max_deg, 8.0),
+
     AP_GROUPEND
 };
 
@@ -612,3 +621,27 @@ void AP_MotorsHeli::set_rotor_runup_complete(bool new_value)
 #endif
     _heliflags.rotor_runup_complete = new_value;
 }
+
+// Helper to calculate the blade pitch angle contribution of the collective
+float AP_MotorsHeli::calculate_collective_blade_angle(float output) const
+{
+    return (_collective_max_deg.get() - _collective_min_deg.get()) * output + _collective_min_deg.get();
+}
+
+// Return the current collective blade pitch angle contribution for swashplate instance
+float AP_MotorsHeli::get_coll_angle(uint8_t inst) const
+{
+    if (inst >= AP_MOTORS_HELI_MAX_INDEPENDENT_COLL){
+        return 0.0;
+    }
+    return _coll_ang_deg[inst]; 
+}
+
+// Return the current cyclic blade pitch angle contribution for swashplate instance
+float AP_MotorsHeli::get_cyc_angle(uint8_t inst) const
+{
+    if (inst >= AP_MOTORS_HELI_MAX_INDEPENDENT_CYCLIC){
+        return 0.0;
+    }
+    return _cyclic_ang_deg[inst]; 
+}

libraries/AP_Motors/AP_MotorsHeli.h

@@ -26,6 +26,8 @@
 #define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG       90.0f // maximum collective blade pitch angle in deg
 #define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN      -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
 
+#define AP_MOTORS_HELI_MAX_INDEPENDENT_COLL     4     // The maximum number of independent collective outputs we can currently mix
+#define AP_MOTORS_HELI_MAX_INDEPENDENT_CYCLIC   2     // The maximum number of independent cyclic outputs we can currently mix
 
 // flybar types
 #define AP_MOTORS_HELI_NOFLYBAR                 0
@@ -142,6 +144,15 @@ class AP_MotorsHeli : public AP_Motors {
 	//return zero lift collective position
     float get_coll_mid() const { return _collective_zero_thrust_pct; }
 
+    // Helper to calculate the blade pitch angle contribution of the collective
+    float calculate_collective_blade_angle(float output) const;
+
+    // Return the current collective blade pitch angle contribution for swashplate instance
+    float get_coll_angle(uint8_t inst) const;
+
+    // Return the current cyclic blade pitch angle contribution for swashplate instance
+    float get_cyc_angle(uint8_t inst) const;
+
     // enum for heli optional features
     enum class HeliOption {
         USE_LEAKY_I                     = (1<<0),   // 1
@@ -272,11 +283,11 @@ class AP_MotorsHeli : public AP_Motors {
     AP_Float        _collective_land_min_deg;   // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
     AP_Float        _collective_max_deg;        // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX)
     AP_Float        _collective_min_deg;        // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
+    AP_Float        _cyclic_angle_max_deg;      // The blade pitch angle angle contribution in degrees of the cyclic output contribution
 
     // internal variables
-    float           _collective_zero_thrust_pct;      // collective zero thrutst parameter value converted to 0 ~ 1 range
-    float           _collective_land_min_pct;      // collective land min parameter value converted to 0 ~ 1 range
-    uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
+    float           _coll_ang_deg[AP_MOTORS_HELI_MAX_INDEPENDENT_COLL];      // collective blade pitch angle contributions for each swashplate after mixing
+    float           _cyclic_ang_deg[AP_MOTORS_HELI_MAX_INDEPENDENT_CYCLIC];  // cyclic blade pitch angle contributions for each swashplate after mixing
 
     motor_frame_type _frame_type;
     motor_frame_class _frame_class;

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -316,6 +316,12 @@ void AP_MotorsHeli_Dual::mix_tandem(float pitch_input, float roll_input, float y
     const float swash1_coll =  0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input;
     const float swash2_coll = -0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
 
+    // Calculate the blade pitch contributions for logging
+    _coll_ang_deg[0] = calculate_collective_blade_angle(swash1_coll);
+    _coll_ang_deg[1] = calculate_collective_blade_angle(swash2_coll);
+    _cyclic_ang_deg[0] = swash1_roll*_cyclic_angle_max_deg.get();
+    _cyclic_ang_deg[1] = swash2_roll*_cyclic_angle_max_deg.get();
+
     // Calculate servo positions in swashplate library
     _swashplate1.calculate(swash1_roll, swash_pitch, swash1_coll);
     _swashplate2.calculate(swash2_roll, swash_pitch, swash2_coll);
@@ -335,6 +341,12 @@ void AP_MotorsHeli_Dual::mix_transverse(float pitch_input, float roll_input, flo
     const float swash1_coll =  0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input;
     const float swash2_coll = -0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
 
+    // Calculate the blade pitch contributions for logging
+    _coll_ang_deg[0] = calculate_collective_blade_angle(swash1_coll);
+    _coll_ang_deg[1] = calculate_collective_blade_angle(swash2_coll);
+    _cyclic_ang_deg[0] = swash1_pitch*_cyclic_angle_max_deg.get();
+    _cyclic_ang_deg[1] = swash2_pitch*_cyclic_angle_max_deg.get();
+
     // Calculate servo positions in swashplate library
     _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
     _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
@@ -354,6 +366,12 @@ void AP_MotorsHeli_Dual::mix_intermeshing(float pitch_input, float roll_input, f
     const float swash1_coll =   0.45 * _dcp_scaler * yaw_input + collective1_input;
     const float swash2_coll =  -0.45 * _dcp_scaler * yaw_input + collective2_input;
 
+    // Calculate the blade pitch contributions for logging
+    _coll_ang_deg[0] = calculate_collective_blade_angle(swash1_coll);
+    _coll_ang_deg[1] = calculate_collective_blade_angle(swash2_coll);
+    _cyclic_ang_deg[0] = norm(swash1_pitch, swash_roll)*_cyclic_angle_max_deg.get();
+    _cyclic_ang_deg[1] = norm(swash2_pitch, swash_roll)*_cyclic_angle_max_deg.get();
+
     // Calculate servo positions in swashplate library
     _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
     _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -254,6 +254,10 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
         // scale output to 0 to 1
         _out[i] += _collective_zero_thrust_pct;
+
+        // Log blade pitch angles for collective outputs
+        _coll_ang_deg[i] = calculate_collective_blade_angle(_out[i]);
+
         // scale output to -1 to 1 for servo output
         _out[i] = _out[i] * 2.0f - 1.0f;
     }

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -388,8 +388,9 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     // coming into this equation at 4500 or less
     float total_out = norm(pitch_out, roll_out);
 
+    float ratio = 1.0;
     if (total_out > (_cyclic_max/4500.0f)) {
-        float ratio = (float)(_cyclic_max/4500.0f) / total_out;
+        ratio = (float)(_cyclic_max/4500.0f) / total_out;
         roll_out *= ratio;
         pitch_out *= ratio;
         limit.roll = true;
@@ -441,6 +442,10 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
 
     // update the yaw rate using the tail rotor/servo
     move_yaw(yaw_out + yaw_offset);
+
+    // calculate the blade pitch angle contribution from collective and cyclic (logging only)
+    _coll_ang_deg[0] = calculate_collective_blade_angle(collective_out);
+    _cyclic_ang_deg[0] = total_out*ratio*_cyclic_angle_max_deg.get();
 }
 
 // move_yaw