AP_Motors_Heli: Add support for swashplate logging

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -612,3 +612,56 @@ 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();
+}
+
+// Helper to get blade pitch angle contributions from swashplate inputs
+bool AP_MotorsHeli::get_swash_angles(uint8_t i, float& col, float& tcyc, float& pcyc, float& rcyc) const
+{
+    if (i > MAX_SWASHPLATES) {
+        return false;
+    }
+
+    if (!swashplate_log[i].used) {
+        return false;
+    }
+
+    col = swashplate_log[i].coll_ang_deg;
+    tcyc = swashplate_log[i].total_cyclic_ang_deg;
+    pcyc = swashplate_log[i].pitch_cyclic_ang_deg;
+    rcyc = swashplate_log[i].roll_cyclic_ang_deg;
+    return true;
+}
+
+// Calculate and log the blade pitch angle collective and cyclic contributions
+void AP_MotorsHeli::log_swashplate(uint8_t i, float sw_roll, float sw_pitch, float collective, float swash_range_scaler)
+{
+    if (i > MAX_SWASHPLATES) {
+        return;
+    }
+
+    // We have received data for this instance of swashplate sp set used to true to ensure we log this instance
+    swashplate_log[i].used = true;
+
+    // We want to use the collective min-max to angle relationship to calculate the cyclic output to angle relationship
+    // First we scale the collective angle range by it's min-max output.  Recall that we assume that the maximum possible 
+    // collective range is 1000, hence the *1e-3.
+    float blade_pitch_angle_scaler = ((float)(_collective_max-_collective_min))*1e-3 * (_collective_max_deg.get() - _collective_min_deg.get());
+
+    // Determine the magnitude of the cyclic input by calculating the length of the roll and pitch outputs
+    float cyclic_mag = norm(sw_roll, sw_pitch);
+
+    // Calculate the cyclic blade angle contribution. The factor 2.0 accounts for the fact that we scale the servo outputs from 0~1 to -1~1
+    // The swash_range_scaler allows us to account for the differences in definition of when cyclic_max is achieved, between single and dual frame types
+    float cyclic_scalar = 2.0 * blade_pitch_angle_scaler * swash_range_scaler;
+    swashplate_log[i].total_cyclic_ang_deg = cyclic_mag * cyclic_scalar;
+    swashplate_log[i].pitch_cyclic_ang_deg = sw_roll * cyclic_scalar;
+    swashplate_log[i].roll_cyclic_ang_deg = sw_pitch * cyclic_scalar;
+
+    // Calculate the collective contribution
+    swashplate_log[i].coll_ang_deg = calculate_collective_blade_angle(collective);
+}

libraries/AP_Motors/AP_MotorsHeli.h

@@ -142,6 +142,12 @@ 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;
+
+    // Helper to get blade pitch angle contributions from swashplate inputs
+    bool get_swash_angles(uint8_t i, float& col, float& tcyc, float& pcyc, float& rcyc) const;
+
     // enum for heli optional features
     enum class HeliOption {
         USE_LEAKY_I                     = (1<<0),   // 1
@@ -165,6 +171,9 @@ class AP_MotorsHeli : public AP_Motors {
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
+    // The maximum number of swashplates we can currently expect any heli to have
+    static const uint8_t MAX_SWASHPLATES = 4;
+
 protected:
 
     // manual servo modes (used for setup)
@@ -236,6 +245,9 @@ class AP_MotorsHeli : public AP_Motors {
     // Update _heliflags.rotor_runup_complete value writing log event on state change
     void set_rotor_runup_complete(bool new_value);
 
+    // Calculate and store blade pitch angle contributions in the swashplate log
+    void log_swashplate(uint8_t i, float sw_roll, float sw_pitch, float collective, float swash_range_scaler);
+
     // enum values for HOVER_LEARN parameter
     enum HoverLearn {
         HOVER_LEARN_DISABLED = 0,
@@ -259,6 +271,14 @@ class AP_MotorsHeli : public AP_Motors {
         uint8_t start_engine            : 1;    // true if turbine start RC option is initiated
     } _heliflags;
 
+    struct AP_MotorsHeli_Swash_Log {
+        bool used;                   // flag to denote if we are using this instance of swashplate for a given frame type
+        float coll_ang_deg;          // collective blade pitch angle contributions for swashplate after mixing
+        float total_cyclic_ang_deg;  // total cyclic blade pitch angle contributions for each swashplate after mixing
+        float pitch_cyclic_ang_deg;  // pitch cyclic blade pitch angle contributions for each swashplate after mixing
+        float roll_cyclic_ang_deg;   // roll cyclic blade pitch angle contributions for each swashplate after mixing
+    } swashplate_log[MAX_SWASHPLATES];
+
     // parameters
     AP_Int16        _cyclic_max;                // Maximum cyclic angle of the swash plate in centi-degrees
     AP_Int16        _collective_min;            // Lowest possible servo position for the swashplate

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -316,6 +316,10 @@ 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;
 
+    // Log blade pitch angle contributions
+    log_swashplate(0, swash1_roll, swash_pitch, swash1_coll, 1.0);
+    log_swashplate(1, swash2_roll, swash_pitch, swash2_coll, 1.0);
+
     // Calculate servo positions in swashplate library
     _swashplate1.calculate(swash1_roll, swash_pitch, swash1_coll);
     _swashplate2.calculate(swash2_roll, swash_pitch, swash2_coll);
@@ -335,6 +339,10 @@ 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;
 
+    // Log blade pitch angle contributions
+    log_swashplate(0, swash_roll, swash1_pitch, swash1_coll, 1.0);
+    log_swashplate(1, swash_roll, swash2_pitch, swash2_coll, 1.0);
+
     // Calculate servo positions in swashplate library
     _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
     _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
@@ -354,6 +362,10 @@ 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;
 
+    // Log blade pitch angle contributions
+    log_swashplate(0, swash_roll, swash1_pitch, swash1_coll, 1.0);
+    log_swashplate(1, swash_roll, swash2_pitch, swash2_coll, 1.0);
+
     // 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 angle contributions
+        log_swashplate(i, 0.0, 0.0, _out[i], 1.0);
+
         // scale output to -1 to 1 for servo output
         _out[i] = _out[i] * 2.0f - 1.0f;
     }

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -436,7 +436,10 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
     float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
 
-    // Caculate servo positions from swashplate library
+    // log the blade pitch contributions
+    log_swashplate(0, roll_out, pitch_out, collective_out, sqrtf(2.0));
+
+    // Calculate servo positions from swashplate library
     _swashplate.calculate(roll_out, pitch_out, collective_out_scaled);
 
     // update the yaw rate using the tail rotor/servo