diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index c58837962d..0ea5d123e1 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -81,7 +81,7 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) if (plane.quadplane.available()) { // setup AP_Motors outputs for failsafe uint16_t mask = plane.quadplane.motors->get_motor_mask(); - hal.rcout->set_failsafe_pwm(mask, plane.quadplane.thr_min_pwm); + hal.rcout->set_failsafe_pwm(mask, plane.quadplane.motors->get_pwm_output_min()); } } diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 55aca13089..95b7a6d4cf 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -42,6 +42,9 @@ void QuadPlane::motor_test_output() int16_t pwm = 0; // pwm that will be output to the motors // calculate pwm based on throttle type + const int16_t thr_min_pwm = motors->get_pwm_output_min(); + const int16_t thr_max_pwm = motors->get_pwm_output_max(); + switch (motor_test.throttle_type) { case MOTOR_TEST_THROTTLE_PERCENT: // sanity check motor_test.throttle value diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c8eacb17d5..6bdd576b03 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -79,23 +79,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("RC_SPEED", 21, QuadPlane, rc_speed, 490), - // @Param: THR_MIN_PWM - // @DisplayName: Minimum PWM output - // @Description: This is the minimum PWM output for the quad motors - // @Units: PWM - // @Range: 800 2200 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("THR_MIN_PWM", 22, QuadPlane, thr_min_pwm, 1000), - - // @Param: THR_MAX_PWM - // @DisplayName: Maximum PWM output - // @Description: This is the maximum PWM output for the quad motors - // @Units: PWM - // @Range: 800 2200 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("THR_MAX_PWM", 23, QuadPlane, thr_max_pwm, 2000), + // 22: THR_MIN_PWM + // 23: THR_MAX_PWM // @Param: ASSIST_SPEED // @DisplayName: Quadplane assistance speed @@ -632,6 +617,10 @@ const AP_Param::ConversionInfo q_conversion_table[] = { { Parameters::k_param_q_attitude_control, 449, AP_PARAM_FLOAT, "Q_A_RAT_RLL_FF" }, // Q_A_RAT_RLL_FF { Parameters::k_param_q_attitude_control, 450, AP_PARAM_FLOAT, "Q_A_RAT_PIT_FF" }, // Q_A_RAT_PIT_FF { Parameters::k_param_q_attitude_control, 451, AP_PARAM_FLOAT, "Q_A_RAT_YAW_FF" }, // Q_A_RAT_YAW_FILT + + { Parameters::k_param_quadplane, 22, AP_PARAM_INT16, "Q_M_PWM_MIN" }, + { Parameters::k_param_quadplane, 23, AP_PARAM_INT16, "Q_M_PWM_MAX" }, + }; @@ -821,7 +810,7 @@ bool QuadPlane::setup(void) // failsafe will disable motors for (uint8_t i=0; i<8; i++) { SRV_Channel::Aux_servo_function_t func = SRV_Channels::get_motor_function(i); - SRV_Channels::set_failsafe_pwm(func, thr_min_pwm); + SRV_Channels::set_failsafe_pwm(func, motors->get_pwm_output_min()); } transition_state = TRANSITION_DONE; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5e28d22f4c..e6499b1d1f 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -328,10 +328,6 @@ private: AP_Int16 rc_speed; - // min and max PWM for throttle - AP_Int16 thr_min_pwm; - AP_Int16 thr_max_pwm; - // speed below which quad assistance is given AP_Float assist_speed;