From 849a93c7173f9bcc3505719543c0707dd6e06dd7 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 5 Jan 2025 16:18:49 +0000 Subject: [PATCH] AP_Motors: allow external slew limit in `output_motor_mask` --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 23 ++++++++++++++++++-- libraries/AP_Motors/AP_MotorsMulticopter.h | 2 +- libraries/AP_Motors/AP_MotorsTri.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsTri.h | 2 +- 4 files changed, 25 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e8a099998a9b73..42d7b17ef46ad6 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 7e8491c8497c07..8089fa41ed579a 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 e387694c2010cf..1b66ec11d15439 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 3c034ccc1ceeb7..e3e9a109cdcfc5 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