diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index bc9d5c05ae00b1..236e4b99378401 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -612,3 +612,12 @@ void AP_MotorsHeli::set_rotor_runup_complete(bool new_value) #endif _heliflags.rotor_runup_complete = new_value; } + +// 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; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 172fd39c6166ef..388e3f2e7f1909 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -236,6 +236,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); + // Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion + float get_cyclic_angle_scaler(void) const; + // 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 c9b645056d01ec..dfcca4b53aa7c8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -594,3 +594,10 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const return true; } + +// 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()); + _swashplate2.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get()); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 46672faddb3004..81aed533da245e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -56,6 +56,9 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli { // Run arming checks bool arming_checks(size_t buflen, char *buffer) const override; + // Blade angle logging - called at 10 Hz + void Log_Write(void) override; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -76,8 +79,8 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli { 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 a996fb7d04cf0d..435fbbb2405b9f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -691,3 +691,12 @@ bool AP_MotorsHeli_Single::use_tail_RSC() const return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) || (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV); } + +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()); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 61e6588a5ab9fb..432e43ad1d944d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -33,7 +33,7 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { 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,9 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { // Thrust Linearization handling Thrust_Linearization thr_lin {*this}; + // Blade angle logging - called at 10 Hz + void Log_Write(void) override; + // 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 3471edc168cc90..f80b7d12862605 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp @@ -87,12 +87,13 @@ 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) { _motor_num[0] = mot_0; _motor_num[1] = mot_1; _motor_num[2] = mot_2; _motor_num[3] = mot_3; + _instance = instance; AP_Param::setup_object_defaults(this, var_info); } @@ -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 = collective; + for (uint8_t i = 0; i < _max_num_servos; i++) { if (!_enabled[i]) { // This servo is not enabled @@ -283,3 +289,31 @@ uint32_t AP_MotorsHeli_Swash::get_output_mask() const } return mask; } + +// 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) const +{ +#if HAL_LOGGING_ENABLED + + float col = (col_ang_max - col_ang_min) * _collective_input + col_ang_min; + 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 927777e98372eb..2478ba97d42695 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,9 @@ class AP_MotorsHeli_Swash { // Get function output mask uint32_t get_output_mask() const; + // Write SWSH log for this instance of swashplate + void write_log(float cyclic_scaler, float col_ang_min, float col_ang_max) const; + // var_info static const struct AP_Param::GroupInfo var_info[]; @@ -76,6 +80,12 @@ class AP_MotorsHeli_Swash { 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 + uint8_t _instance; // Swashplate instance. Used for logging. + + // Variables stored for logging + float _roll_input; + float _pitch_input; + float _collective_input; // parameters AP_Int8 _swashplate_type; // Swash Type Setting