diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index bc9d5c05ae..d81da48b1a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 172fd39c61..472e86d01a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/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, diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index c9b645056d..3bc2ab84e7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 46672faddb..2fb696eb91 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index a996fb7d04..33cc445904 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 61e6588a5a..72754a1134 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/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[]; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp index 3471edc168..78c126890f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.h b/libraries/AP_Motors/AP_MotorsHeli_Swash.h index 927777e983..cde9fde23f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.h @@ -5,6 +5,7 @@ #include #include // ArduPilot Mega Vector/Matrix math Library #include +#include // 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