diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b272a13b11ff1..a22d9fdb2c803 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1180,6 +1180,7 @@ class Plane : public AP_Vehicle { void servos_twin_engine_mix(); void force_flare(); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); + float get_throttle_slewrate() const; void throttle_slew_limit(); bool suppress_throttle(void); void update_throttle_hover(); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2ded238cf2f5b..1c1f27f5add17 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -19,10 +19,10 @@ #include "Plane.h" #include -/***************************************** -* Throttle slew limit -*****************************************/ -void Plane::throttle_slew_limit() +// Get slew rate for throttle as a percentage per second. +// 100% would allow the throttle to move from 0 to 100 in 1 second. +// 0 is disabled +float Plane::get_throttle_slewrate() const { #if HAL_QUADPLANE_ENABLED const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode()); @@ -32,10 +32,7 @@ void Plane::throttle_slew_limit() if (!do_throttle_slew) { // only do throttle slew limiting in modes where throttle control is automatic - SRV_Channels::set_slew_rate(SRV_Channel::k_throttle, 0.0, 100, G_Dt); - SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft, 0.0, 100, G_Dt); - SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, 0.0, 100, G_Dt); - return; + return 0.0; } uint8_t slewrate = aparm.throttle_slewrate; @@ -57,6 +54,13 @@ void Plane::throttle_slew_limit() slewrate = g.takeoff_throttle_slewrate; } #endif + return slewrate; +} + +// Apply throttle slew limit +void Plane::throttle_slew_limit() +{ + const float slewrate = get_throttle_slewrate(); SRV_Channels::set_slew_rate(SRV_Channel::k_throttle, slewrate, 100, G_Dt); SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft, slewrate, 100, G_Dt); SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, slewrate, 100, G_Dt); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 759fa3d1a4468..58ca7e389f991 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -336,7 +336,7 @@ void Tailsitter::output(void) if (!quadplane.assisted_flight) { // set AP_MotorsMatrix throttles for forward flight - motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt); + motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt, plane.get_throttle_slewrate()); // No tilt output unless forward gain is set float tilt_left = 0.0; diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index fe893b01cd730..e373e4584e767 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -246,7 +246,7 @@ void Tiltrotor::continuous_update(void) if (!quadplane.motor_test.running) { // the motors are all the way forward, start using them for fwd thrust const uint16_t mask = is_zero(current_throttle)?0U:tilt_mask.get(); - motors->output_motor_mask(current_throttle, mask, plane.rudder_dt); + motors->output_motor_mask(current_throttle, mask, plane.rudder_dt, plane.get_throttle_slewrate()); } return; } @@ -367,7 +367,7 @@ void Tiltrotor::binary_update(void) if (current_tilt >= 1) { const uint16_t mask = is_zero(new_throttle)?0U:tilt_mask.get(); // the motors are all the way forward, start using them for fwd thrust - motors->output_motor_mask(new_throttle, mask, plane.rudder_dt); + motors->output_motor_mask(new_throttle, mask, plane.rudder_dt, plane.get_throttle_slewrate()); } } else { binary_slew(false); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e8a099998a9b7..42d7b17ef46ad 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -740,13 +740,18 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th // output a thrust to all motors that match a given motor mask. This // is used to control tiltrotor motors in forward flight. Thrust is in // the range 0 to 1 -void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float rudder_dt) +void AP_MotorsMulticopter::output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit) { const int16_t pwm_min = get_pwm_output_min(); const int16_t pwm_range = get_pwm_output_max() - pwm_min; _motor_mask_override = mask; + // Convert from percentage per second to change per loop. + // Negative or zero is disabled + const float max_change = slew_limit * 0.01 * _dt; + const bool apply_slew_limit = is_positive(max_change); + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i] && (mask & (1U << i)) != 0) { if (armed() && get_interlock()) { @@ -755,8 +760,22 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float copter frame roll is plane frame yaw as this only apples to either tilted motors or tailsitters */ - float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f; + const float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5; + + // Apply slew limiting, this is the lower of the motors slew limiting and the slew limit passed in. + const float last = _actuator[i]; + + // Apply motor slew limits set_actuator_with_slew(_actuator[i], thrust + diff_thrust); + + // Apply passed in slew limit + if (apply_slew_limit) { + const float lower = MAX(0.0, last - max_change); + const float upper = MIN(1.0, last + max_change); + + _actuator[i] = constrain_float(_actuator[i], lower, upper); + } + } else { // zero throttle _actuator[i] = 0.0; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 7e8491c8497c0..8089fa41ed579 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -60,7 +60,7 @@ class AP_MotorsMulticopter : public AP_Motors { // output a thrust to all motors that match a given motor // mask. This is used to control tiltrotor motors in forward // flight. Thrust is in the range 0 to 1 - virtual void output_motor_mask(float thrust, uint16_t mask, float rudder_dt); + virtual void output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit); // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index e387694c2010c..1b66ec11d1543 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -351,10 +351,10 @@ void AP_MotorsTri::thrust_compensation(void) /* override tricopter tail servo output in output_motor_mask */ -void AP_MotorsTri::output_motor_mask(float thrust, uint16_t mask, float rudder_dt) +void AP_MotorsTri::output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit) { // normal multicopter output - AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt); + AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt, slew_limit); // and override yaw servo rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 3c034ccc1ceeb..e3e9a109cdcfc 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -42,7 +42,7 @@ class AP_MotorsTri : public AP_MotorsMulticopter { // mask. This is used to control tiltrotor motors in forward // flight. Thrust is in the range 0 to 1 // rudder_dt applys diffential thrust for yaw in the range 0 to 1 - void output_motor_mask(float thrust, uint16_t mask, float rudder_dt) override; + void output_motor_mask(const float thrust, const uint16_t mask, const float rudder_dt, const float slew_limit) override; // return the roll factor of any motor, this is used for tilt rotors and tail sitters // using copter motors for forward flight