AP_MotorsHeli: Add collective and cyclic blade pitch angle logging

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -612,3 +612,14 @@ void AP_MotorsHeli::set_rotor_runup_complete(bool new_value)
 #endif
     _heliflags.rotor_runup_complete = new_value;
 }
+
+#if HAL_LOGGING_ENABLED
+// Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion
+float AP_MotorsHeli::get_cyclic_angle_scaler(void) const {
+    // We want to use the collective min-max to angle relationship to calculate the cyclic input 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.
+    // The factor 2.0 accounts for the fact that we scale the servo outputs from 0 ~ 1 to -1 ~ 1
+    return ((float)(_collective_max-_collective_min))*1e-3 * (_collective_max_deg.get() - _collective_min_deg.get()) * 2.0;
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli.h

@@ -236,6 +236,11 @@ protected:
     // Update _heliflags.rotor_runup_complete value writing log event on state change
     void set_rotor_runup_complete(bool new_value);
 
+#if HAL_LOGGING_ENABLED
+    // Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion for blade angle logging
+    float get_cyclic_angle_scaler(void) const;
+#endif
+
     // enum values for HOVER_LEARN parameter
     enum HoverLearn {
         HOVER_LEARN_DISABLED = 0,

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -594,3 +594,12 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
 
     return true;
 }
+
+#if HAL_LOGGING_ENABLED
+// Blade angle logging - called at 10 Hz
+void AP_MotorsHeli_Dual::Log_Write(void)
+{
+    _swashplate1.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
+    _swashplate2.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective2_min.get(), _collective2_max.get());
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -56,6 +56,11 @@ public:
     // Run arming checks
     bool arming_checks(size_t buflen, char *buffer) const override;
 
+#if HAL_LOGGING_ENABLED
+    // Blade angle logging - called at 10 Hz
+    void Log_Write(void) override;
+#endif
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -76,8 +81,8 @@ protected:
     const char* _get_frame_string() const override { return "HELI_DUAL"; }
 
     //  objects we depend upon
-    AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7 }; // swashplate1
-    AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8 }; // swashplate2
+    AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7, 1U }; // swashplate1
+    AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8, 2U }; // swashplate2
 
     // internal variables
     float _oscillate_angle = 0.0f;                  // cyclic oscillation angle, used by servo_test function

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -691,3 +691,14 @@ bool AP_MotorsHeli_Single::use_tail_RSC() const
     return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) ||
            (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV);
 }
+
+#if HAL_LOGGING_ENABLED
+void AP_MotorsHeli_Single::Log_Write(void)
+{
+    // For single heli we have to apply an additional cyclic scaler of sqrt(2.0) because the
+    // definition of when we achieve _cyclic_max is different to dual heli. In single, _cyclic_max
+    // is limited at sqrt(2.0), in dual it is limited at 1.0
+    float cyclic_angle_scaler = get_cyclic_angle_scaler() * sqrtf(2.0);
+    _swashplate.write_log(cyclic_angle_scaler, _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -33,7 +33,7 @@ public:
     AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
         AP_MotorsHeli(speed_hz),
         _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC),
-        _swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5)
+        _swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5, 1U)
     {
         AP_Param::setup_object_defaults(this, var_info);
     };
@@ -77,6 +77,11 @@ public:
     // Thrust Linearization handling
     Thrust_Linearization thr_lin {*this};
 
+#if HAL_LOGGING_ENABLED
+    // Blade angle logging - called at 10 Hz
+    void Log_Write(void) override;
+#endif
+
     // var_info
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -87,7 +87,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
     AP_GROUPEND
 };
 
-AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3)
+AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance) :
+    _instance(instance)
 {
     _motor_num[0] = mot_0;
     _motor_num[1] = mot_1;
@@ -219,6 +220,11 @@ void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
         collective = 1 - collective;
     }
 
+    // Store inputs for logging
+    _roll_input = roll;
+    _pitch_input = pitch;
+    _collective_input_scaled = collective;
+
     for (uint8_t i = 0; i < _max_num_servos; i++) {
         if (!_enabled[i]) {
             // This servo is not enabled
@@ -283,3 +289,37 @@ uint32_t AP_MotorsHeli_Swash::get_output_mask() const
     }
     return mask;
 }
+
+#if HAL_LOGGING_ENABLED
+// Write SWSH log for this instance of swashplate
+void AP_MotorsHeli_Swash::write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const
+{
+    // Calculate the collective contribution to blade pitch angle
+    // Swashplate receives the scaled collective value based on the col_min and col_max params. We have to reverse the scaling here to do the angle calculation.
+    float collective_scalar = ((float)(col_max-col_min))*1e-3;
+    collective_scalar = MAX(collective_scalar, 1e-3);
+    float _collective_input = (_collective_input_scaled - (float)(col_min - 1000)*1e-3) / collective_scalar;
+    float col = (col_ang_max - col_ang_min) * _collective_input + col_ang_min;
+
+    // Calculate the cyclic contribution to blade pitch angle
+    float tcyc = norm(_roll_input, _pitch_input) * cyclic_scaler;
+    float pcyc = _pitch_input * cyclic_scaler;
+    float rcyc = _roll_input * cyclic_scaler;
+
+    // @LoggerMessage: SWSH
+    // @Description: Helicopter swashplate logging
+    // @Field: TimeUS: Time since system startup
+    // @Field: I: Swashplate instance
+    // @Field: Col: Blade pitch angle contribution from collective
+    // @Field: TCyc: Total blade pitch angle contribution from cyclic
+    // @Field: PCyc: Blade pitch angle contribution from pitch cyclic
+    // @Field: RCyc: Blade pitch angle contribution from roll cyclic
+    AP::logger().WriteStreaming("SWSH", "TimeUS,I,Col,TCyc,PCyc,RCyc", "s#dddd", "F-0000", "QBffff",
+                                AP_HAL::micros64(),
+                                _instance,
+                                col,
+                                tcyc,
+                                pcyc,
+                                rcyc);
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -5,6 +5,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <AP_Param/AP_Param.h>
+#include <AP_Logger/AP_Logger.h>
 
 // swashplate types
 enum SwashPlateType {
@@ -19,7 +20,7 @@ enum SwashPlateType {
 class AP_MotorsHeli_Swash {
 public:
 
-    AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3);
+    AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance);
 
     // configure - configure the swashplate settings for any updated parameters
     void configure();
@@ -39,6 +40,11 @@ public:
     // Get function output mask
     uint32_t get_output_mask() const;
 
+#if HAL_LOGGING_ENABLED
+    // Write SWSH log for this instance of swashplate
+    void write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const;
+#endif
+
     // var_info
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -76,6 +82,12 @@ private:
     float                _collectiveFactor[_max_num_servos];        // Collective axis scaling of servo output based on servo position
     float                _output[_max_num_servos];                  // Servo output value
     uint8_t              _motor_num[_max_num_servos];               // Motor function to use for output
+    const uint8_t        _instance;                                 // Swashplate instance. Used for logging.
+
+    // Variables stored for logging
+    float _roll_input;
+    float _pitch_input;
+    float _collective_input_scaled;
 
     // parameters
     AP_Int8  _swashplate_type;                   // Swash Type Setting