diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index b06e27da49..e913fb01bd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -493,6 +493,22 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const return false; } + // returns false if _collective_min_deg is not default value which indicates users set parameter + if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) { + if (display_msg) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MIN_DEG to actual min blade pitch in deg"); + } + return false; + } + + // returns false if _collective_max_deg is not default value which indicates users set parameter + if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) { + if (display_msg) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MAX_DEG to actual max blade pitch in deg"); + } + return false; + } + // all other cases parameters are OK return true; } diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 2e0d609a79..38d00b7691 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -22,8 +22,8 @@ #define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1 #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle -#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -10.0f // minimum collective blade pitch angle in deg -#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 10.0f // maximum collective blade pitch angle in deg +#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -90.0f // minimum collective blade pitch angle in deg +#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 @@ -267,8 +267,8 @@ protected: 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) // internal variables - float _collective_zero_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range - float _collective_land_min_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range + float _collective_zero_pct; // collective mid parameter value converted to 0 ~ 1 range + float _collective_land_min_pct; // collective mid parameter value converted to 0 ~ 1 range uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup motor_frame_type _frame_type; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 60ca95f9fe..c3ec056455 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -377,14 +377,20 @@ void AP_MotorsHeli_Dual::calculate_scalars() _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); - // calculate collective zero thrust point as a number from 0 to 1 - _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); - _collective2_zero_pct = _collective_zero_pct; - _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); - // calculate collective land min point as a number from 0 to 1 - _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { + // calculate collective zero thrust point as a number from 0 to 1 + _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + + // calculate collective land min point as a number from 0 to 1 + _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + } else { + _collective_zero_pct = 0.0f; + _collective_land_min_pct = 0.0f; + } + + _collective2_zero_pct = _collective_zero_pct; // configure swashplate 1 and update scalars _swashplate1.configure(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 00a35f91c3..55da1c237c 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -149,13 +149,18 @@ void AP_MotorsHeli_Quad::calculate_scalars() _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); - // calculate collective zero thrust point as a number from 0 to 1 - _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); - _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); - // calculate collective land min point as a number from 0 to 1 - _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { + // calculate collective zero thrust point as a number from 0 to 1 + _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + + // calculate collective land min point as a number from 0 to 1 + _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + } else { + _collective_zero_pct = 0.0f; + _collective_land_min_pct = 0.0f; + } // calculate factors based on swash type and servo position calculate_roll_pitch_collective_factors(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index fc1d7e8bb5..c8f0aa4edd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -324,15 +324,21 @@ void AP_MotorsHeli_Single::calculate_scalars() _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN; _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX; } - _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); - // calculate collective zero thrust point as a number from 0 to 1 - _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + _collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg); _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); - // calculate collective land min point as a number from 0 to 1 - _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { + // calculate collective zero thrust point as a number from 0 to 1 + _collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + + // calculate collective land min point as a number from 0 to 1 + _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); + } else { + _collective_zero_pct = 0.0f; + _collective_land_min_pct = 0.0f; + } // configure swashplate and update scalars _swashplate.configure();