diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 9f6b29b2b8..45b00ebab1 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -367,6 +367,9 @@ public: // get scaled output for the given function type. static float get_output_scaled(SRV_Channel::Aux_servo_function_t function); + // get slew limited scaled output for the given function type + static float get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function); + // get pwm output for the first channel of the given function type. static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value); @@ -381,7 +384,7 @@ public: static uint16_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function); // limit slew rate to given limit in percent per second - static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt); + static void set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt); // call output_ch() on all channels static void output_ch_all(void); @@ -620,6 +623,16 @@ private: static bool emergency_stop; + // linked list for slew rate handling + struct slew_list { + slew_list(SRV_Channel::Aux_servo_function_t _func) : func(_func) {}; + const SRV_Channel::Aux_servo_function_t func; + float last_scaled_output; + float max_change; + slew_list * next; + }; + static slew_list *_slew; + // semaphore for multi-thread use of override_counter array HAL_Semaphore override_counter_sem; }; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 81353cfc63..0c94f9f9ce 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -542,6 +542,25 @@ float SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function return 0; } +// get slew limited scaled output for the given function type +float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function) +{ + if (!SRV_Channel::valid_function(function)) { + return 0.0; + } + for (slew_list *slew = _slew; slew; slew = slew->next) { + if (slew->func == function) { + if (!is_positive(slew->max_change)) { + // treat negative or zero slew rate as disabled + break; + } + return constrain_float(functions[function].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change); + } + } + // no slew limiting + return functions[function].output_scaled; +} + /* get mask of output channels for a function */ @@ -691,33 +710,35 @@ void SRV_Channels::set_output_norm(SRV_Channel::Aux_servo_function_t function, f limit slew rate for an output function to given rate in percent per second. This assumes output has not yet done to the hal */ -void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt) +void SRV_Channels::set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt) { - if (slew_rate <= 0) { - // nothing to do - return; - } if (!SRV_Channel::valid_function(function)) { return; } - for (uint8_t i=0; iread_last_sent(c.ch_num); - if (last_pwm == c.get_output_pwm()) { - continue; - } - uint16_t max_change = (c.get_output_max() - c.get_output_min()) * slew_rate * dt * 0.01f; - if (max_change == 0 || dt > 1) { - // always allow some change. If dt > 1 then assume we - // are just starting out, and only allow a small - // change for this loop - max_change = 1; - } - c.set_output_pwm(constrain_int16(c.get_output_pwm(), last_pwm-max_change, last_pwm+max_change)); + const float max_change = range * slew_rate * 0.01 * dt; + + for (slew_list *slew = _slew; slew; slew = slew->next) { + if (slew->func == function) { + // found existing item, update max change + slew->max_change = max_change; + return; } } + + if (!is_positive(max_change)) { + // no point in adding a disabled slew limit + return; + } + + // add new item + slew_list *new_slew = new slew_list(function); + if (new_slew == nullptr) { + return; + } + new_slew->last_scaled_output = functions[function].output_scaled; + new_slew->max_change = max_change; + new_slew->next = _slew; + _slew = new_slew; } // call set_angle() on matching channels diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index a23f6f4550..24fecd606b 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -68,6 +68,7 @@ bool SRV_Channels::initialised; bool SRV_Channels::emergency_stop; Bitmask SRV_Channels::function_mask; SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions]; +SRV_Channels::slew_list *SRV_Channels::_slew; const AP_Param::GroupInfo SRV_Channels::var_info[] = { #if (NUM_SERVO_CHANNELS >= 1) @@ -306,6 +307,15 @@ void SRV_Channels::setup_failsafe_trim_all_non_motors(void) */ void SRV_Channels::calc_pwm(void) { + // slew rate limit functions + for (slew_list *slew = _slew; slew; slew = slew->next) { + if (is_positive(slew->max_change)) { + // treat negative or zero slew rate as disabled + functions[slew->func].output_scaled = constrain_float(functions[slew->func].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change); + } + slew->last_scaled_output = functions[slew->func].output_scaled; + } + WITH_SEMAPHORE(_singleton->override_counter_sem); for (uint8_t i=0; i