diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 447bb6e92a..391a7414ac 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -29,7 +29,7 @@ void AP_MotorsHeli_RSC::init_servo() } // set_power_output_range -void AP_MotorsHeli_RSC::set_power_output_range(uint16_t power_low, uint16_t power_high) +void AP_MotorsHeli_RSC::set_power_output_range(float power_low, float power_high) { _power_output_low = power_low; _power_output_high = power_high; @@ -162,6 +162,13 @@ void AP_MotorsHeli_RSC::write_rsc(float servo_out) // ToDo: We should probably use RC_Channel_Aux to avoid this problem return; } else { - RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_heli_rsc, servo_out * 1000.0f); + // calculate PWM value based on H_RSC_PWM_MIN, H_RSC_PWM_MAX and H_RSC_PWM_REV + uint16_t pwm = servo_out * (_pwm_max - _pwm_min); + if (_pwm_rev >= 0) { + pwm = _pwm_min + pwm; + } else { + pwm = _pwm_max - pwm; + } + RC_Channel_aux::set_radio(_aux_fn, pwm); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index a5b10d17fc..dcb59eb2d8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -23,6 +23,8 @@ enum RotorControlMode { class AP_MotorsHeli_RSC { public: + friend class AP_MotorsHeli_Single; + AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn, uint8_t default_channel, uint16_t loop_rate) : @@ -69,7 +71,7 @@ public: void set_runup_time(int8_t runup_time) { _runup_time = runup_time; } // set_power_output_range - void set_power_output_range(uint16_t power_low, uint16_t power_high); + void set_power_output_range(float power_low, float power_high); // set_motor_load void set_motor_load(float load) { _load_feedforward = load; } @@ -102,6 +104,10 @@ private: float _power_output_range = 0.0f; // maximum range of output power float _load_feedforward = 0.0f; // estimate of motor load, range 0-1.0f + AP_Int16 _pwm_min; + AP_Int16 _pwm_max; + AP_Int8 _pwm_rev; + // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 20b286c221..e88bb40001 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -132,6 +132,27 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Path: ../RC_Channel/RC_Channel.cpp AP_SUBGROUPINFO(_yaw_servo, "SV4_", 15, AP_MotorsHeli_Single, RC_Channel), + // @Param: RSC_PWM_MIN + // @DisplayName: RSC PWM output miniumum + // @Description: This sets the PWM output on RSC channel for maximum rotor speed + // @Range: 0 2000 + // @User: Standard + AP_GROUPINFO("RSC_PWM_MIN", 16, AP_MotorsHeli_Single, _main_rotor._pwm_min, 1000), + + // @Param: RSC_PWM_MAX + // @DisplayName: RSC PWM output maxiumum + // @Description: This sets the PWM output on RSC channel for miniumum rotor speed + // @Range: 0 2000 + // @User: Standard + AP_GROUPINFO("RSC_PWM_MAX", 17, AP_MotorsHeli_Single, _main_rotor._pwm_max, 2000), + + // @Param: RSC_PWM_REV + // @DisplayName: RSC PWM reversal + // @Description: This controls reversal of the RSC channel output + // @Values: -1:Reversed,1:Normal + // @User: Standard + AP_GROUPINFO("RSC_PWM_REV", 18, AP_MotorsHeli_Single, _main_rotor._pwm_rev, 1), + // parameters up to and including 29 are reserved for tradheli AP_GROUPEND