Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SRV_Channel: implement limit_slew_rate()

Browse Source
This commit is contained in:
Andrew Tridgell 2017-01-07 17:13:54 +11:00
parent 51864b23eb
commit ea2e32c102
2 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SRV_Channel/SRV_Channel.h
View File

@ -203,6 +203,8 @@ private:
*/
class SRV_Channels {
public:
friend class SRV_Channel;
// constructor
SRV_Channels(void);
@ -232,7 +234,7 @@ public:
static float get_output_norm(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);
static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt);
// call output_ch() on all channels
static void output_ch_all(void);

22
libraries/SRV_Channel/SRV_Channel_aux.cpp
View File

@ -542,11 +542,27 @@ float SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t function)
}
/*
limit slew rate for an output function to given rate in percent per second
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)
void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt)
{
// NOT IMPLEMENTED YET
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
SRV_Channel &ch = channels[i];
if (ch.function == function) {
ch.calc_pwm(functions[function].output_scaled);
uint16_t last_pwm = hal.rcout->read(ch.ch_num);
if (last_pwm == ch.output_pwm) {
continue;
}
uint16_t max_change = (ch.get_output_max() - ch.get_output_min()) * slew_rate * dt * 0.01f;
if (max_change == 0) {
// always allow some change
max_change = 1;
}
ch.output_pwm = constrain_int16(ch.output_pwm, last_pwm-max_change, last_pwm+max_change);
}
}
}
// call set_angle() on matching channels