diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index cece6dd8cd..5d315b9176 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -87,7 +87,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @Param: PWM_TYPE // @DisplayName: Output PWM type // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output - // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200,8:PWMRange + // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200,8:PWMRange,9:PWMAngle // @User: Advanced // @RebootRequired: True AP_GROUPINFO("PWM_TYPE", 15, AP_MotorsMulticopter, _pwm_type, PWM_TYPE_NORMAL), @@ -474,7 +474,7 @@ void AP_MotorsMulticopter::update_throttle_range() { // if all outputs are digital adjust the range. We also do this for type PWM_RANGE, as those use the // scaled output, which is then mapped to PWM via the SRV_Channel library - if (SRV_Channels::have_digital_outputs(get_motor_mask()) || (_pwm_type == PWM_TYPE_PWM_RANGE)) { + if (SRV_Channels::have_digital_outputs(get_motor_mask()) || (_pwm_type == PWM_TYPE_PWM_RANGE) || (_pwm_type == PWM_TYPE_PWM_ANGLE)) { _pwm_min.set_and_default(1000); _pwm_max.set_and_default(2000); } diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 9fbdf5ea4d..b5d81c13b7 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -105,9 +105,9 @@ void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) { SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan); - if ((1U<set_freq(mask, freq_hz); hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type()); - switch (pwm_type(_pwm_type.get())) { + const pwm_type type = (pwm_type)_pwm_type.get(); + switch (type) { case PWM_TYPE_ONESHOT: if (freq_hz > 50 && mask != 0) { hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT); @@ -162,14 +163,32 @@ void AP_Motors::rc_set_freq(uint32_t motor_mask, uint16_t freq_hz) hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT1200); break; case PWM_TYPE_PWM_RANGE: + case PWM_TYPE_PWM_ANGLE: /* this is a motor output type for multirotors which honours - the SERVOn_MIN/MAX values per channel + the SERVOn_MIN/MAX (and TRIM for angle) values per channel + + Motor PWM min and max are hard coded to 1000 to 2000. + Range type offsets by 1000 to get 0 to 1000. + Angle type offsets by 1500 to get -500 to 500. */ - _motor_pwm_range_mask |= motor_mask; + + if (type == PWM_TYPE_PWM_RANGE) { + _motor_pwm_scaled.offset = 1000.0; + } else { + // PWM_TYPE_PWM_ANGLE + _motor_pwm_scaled.offset = 1500.0; + } + + _motor_pwm_scaled.mask |= motor_mask; for (uint8_t i=0; i<16; i++) { if ((1U<set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_NORMAL); @@ -243,6 +262,7 @@ bool AP_Motors::is_digital_pwm_type() const case PWM_TYPE_ONESHOT125: case PWM_TYPE_BRUSHED: case PWM_TYPE_PWM_RANGE: + case PWM_TYPE_PWM_ANGLE: break; } return false; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 92c62bf054..d33b117909 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -265,7 +265,7 @@ public: bool is_brushed_pwm_type() const { return _pwm_type == PWM_TYPE_BRUSHED; } // returns true is pwm type is normal - bool is_normal_pwm_type() const { return (_pwm_type == PWM_TYPE_NORMAL) || (_pwm_type == PWM_TYPE_PWM_RANGE); } + bool is_normal_pwm_type() const { return (_pwm_type == PWM_TYPE_NORMAL) || (_pwm_type == PWM_TYPE_PWM_RANGE) || (_pwm_type == PWM_TYPE_PWM_ANGLE); } MAV_TYPE get_frame_mav_type() const { return _mav_type; } @@ -333,8 +333,15 @@ protected: // mask of what channels need fast output uint32_t _motor_fast_mask; - // mask of what channels need to use SERVOn_MIN/MAX for output mapping - uint32_t _motor_pwm_range_mask; + // Used with PWM_TYPE_PWM_RANGE and PWM_TYPE_PWM_ANGLE + struct { + // Mask of motors using scaled output + uint32_t mask; + + // Offset used to convert from PWM to scaled value + float offset; + } _motor_pwm_scaled; + // pass through variables float _roll_radio_passthrough; // roll input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed @@ -354,15 +361,18 @@ protected: MAV_TYPE _mav_type; // MAV_TYPE_GENERIC = 0; - enum pwm_type { PWM_TYPE_NORMAL = 0, - PWM_TYPE_ONESHOT = 1, - PWM_TYPE_ONESHOT125 = 2, - PWM_TYPE_BRUSHED = 3, - PWM_TYPE_DSHOT150 = 4, - PWM_TYPE_DSHOT300 = 5, - PWM_TYPE_DSHOT600 = 6, - PWM_TYPE_DSHOT1200 = 7, - PWM_TYPE_PWM_RANGE = 8 }; + enum pwm_type { + PWM_TYPE_NORMAL = 0, + PWM_TYPE_ONESHOT = 1, + PWM_TYPE_ONESHOT125 = 2, + PWM_TYPE_BRUSHED = 3, + PWM_TYPE_DSHOT150 = 4, + PWM_TYPE_DSHOT300 = 5, + PWM_TYPE_DSHOT600 = 6, + PWM_TYPE_DSHOT1200 = 7, + PWM_TYPE_PWM_RANGE = 8, + PWM_TYPE_PWM_ANGLE = 9 + }; // return string corresponding to frame_class virtual const char* _get_frame_string() const = 0;