mirror of https://github.com/ArduPilot/ardupilot
137 lines
4.0 KiB
C++
137 lines
4.0 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
simple model of a servo. Model is:
|
|
|
|
- time delay for transport protocol delay
|
|
- slew limit
|
|
- 2-pole butterworth
|
|
*/
|
|
|
|
#include "ServoModel.h"
|
|
#include "SITL.h"
|
|
|
|
// SITL servo model parameters
|
|
const AP_Param::GroupInfo SITL::SIM::ServoParams::var_info[] = {
|
|
// @Param: SPEED
|
|
// @DisplayName: servo speed
|
|
// @Description: servo speed (time for 60 degree deflection). If DELAY and FILTER are not set then this is converted to a 1p lowpass filter. If DELAY or FILTER are set then this is treated as a rate of change limit
|
|
// @Units: s
|
|
AP_GROUPINFO("SPEED", 1, ServoParams, servo_speed, 0.14),
|
|
|
|
// @Param: DELAY
|
|
// @DisplayName: servo delay
|
|
// @Description: servo delay
|
|
// @Units: s
|
|
AP_GROUPINFO("DELAY", 2, ServoParams, servo_delay, 0.0),
|
|
|
|
// @Param: FILTER
|
|
// @DisplayName: servo filter
|
|
// @Description: servo filter
|
|
// @Units: Hz
|
|
AP_GROUPINFO("FILTER", 3, ServoParams, servo_filter, 0),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
/*
|
|
simpler filter used when SIM_SERVO_FILTER and SIM_SERVO_DELAY are not set
|
|
this filter is a 1p low pass based on SIM_SERVO_SPEED
|
|
*/
|
|
float ServoModel::apply_simple_filter(float v, float dt)
|
|
{
|
|
const auto *sitl = AP::sitl();
|
|
if (!is_positive(sitl->servo.servo_speed)) {
|
|
return v;
|
|
}
|
|
const float cutoff = 1.0f / (2 * M_PI * sitl->servo.servo_speed);
|
|
filter1p.set_cutoff_frequency(cutoff);
|
|
return filter1p.apply(v, dt);
|
|
}
|
|
|
|
/*
|
|
apply a filter for a servo model consistting of a delay, speed and 2p filter
|
|
*/
|
|
float ServoModel::apply_filter(float v, float dt)
|
|
{
|
|
const auto *sitl = AP::sitl();
|
|
if (!sitl) {
|
|
return v;
|
|
}
|
|
|
|
if (!is_positive(sitl->servo.servo_delay) &&
|
|
!is_positive(sitl->servo.servo_filter)) {
|
|
// fallback to a simpler 1p filter model
|
|
return apply_simple_filter(v, dt);
|
|
}
|
|
|
|
// apply delay
|
|
if (sitl->servo.servo_delay > 0) {
|
|
uint32_t delay_len = MAX(1,sitl->servo.servo_delay * sitl->loop_rate_hz);
|
|
if (!delay) {
|
|
delay = NEW_NOTHROW ObjectBuffer<float>();
|
|
}
|
|
if (delay->get_size() != delay_len) {
|
|
delay->set_size(delay_len);
|
|
}
|
|
while (delay->space() > 0) {
|
|
delay->push(v);
|
|
}
|
|
IGNORE_RETURN(delay->pop(v));
|
|
}
|
|
|
|
// apply slew limit
|
|
if (sitl->servo.servo_speed > 0) {
|
|
// assume SIM_SERVO_SPEED is time for 60 degrees
|
|
float time_per_degree = sitl->servo.servo_speed / 60.0;
|
|
float proportion_per_second = 1.0 / (angle_deg * time_per_degree);
|
|
delta_max = proportion_per_second * dt;
|
|
v = constrain_float(v, last_v-delta_max, last_v+delta_max);
|
|
v = constrain_float(v, -1, 1);
|
|
last_v = v;
|
|
}
|
|
|
|
// apply filter
|
|
if (sitl->servo.servo_filter > 0) {
|
|
filter.set_cutoff_frequency(sitl->loop_rate_hz, sitl->servo.servo_filter);
|
|
v = filter.apply(v);
|
|
}
|
|
|
|
return v;
|
|
}
|
|
|
|
float ServoModel::filter_range(uint16_t pwm, float dt)
|
|
{
|
|
const float v = (pwm - pwm_min)/float(pwm_max - pwm_min);
|
|
return apply_filter(v, dt);
|
|
}
|
|
|
|
float ServoModel::filter_angle(uint16_t pwm, float dt)
|
|
{
|
|
const float v = (pwm - 0.5*(pwm_max+pwm_min))/(0.5*float(pwm_max - pwm_min));
|
|
return apply_filter(v, dt);
|
|
}
|
|
|
|
void ServoModel::set_pwm_range(uint16_t _pwm_min, uint16_t _pwm_max)
|
|
{
|
|
pwm_min = _pwm_min;
|
|
pwm_max = _pwm_max;
|
|
}
|
|
|
|
void ServoModel::set_deflection(float _angle_deg)
|
|
{
|
|
angle_deg = fabsf(_angle_deg);
|
|
}
|