diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index bc9d5c05ae00b1..bf8d287129a9a2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/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]; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 172fd39c6166ef..2ac1ff3c832953 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/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; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index c9b645056d01ec..96a40d7089b10f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 817e89f1ac1e7c..2f81aeaf7a75cc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/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 (_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