From ebaf1e28b1610fd57c780a2bb0607fe23ee0cbf3 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 10 Mar 2015 15:23:45 -0700 Subject: [PATCH] AP_Gimbal: smooth the RC input with a low pass filter --- libraries/AP_Gimbal/AP_Gimbal.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp index fac8c98e00..0a06938f11 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.cpp +++ b/libraries/AP_Gimbal/AP_Gimbal.cpp @@ -167,8 +167,13 @@ float angle_input_rad(RC_Channel* rc, float angle_min, float angle_max) // update_targets_from_rc - updates angle targets using input from receiver void AP_Gimbal::update_targets_from_rc() { + // Get new tilt angle float new_tilt = (_rc_failsafe)?0.0f:angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max); + // Low-pass filter + new_tilt = _angle_ef_target_rad.y + 0.09f*(new_tilt - _angle_ef_target_rad.y); + // Slew-rate constrain float max_change_rads =_max_tilt_rate * _measurament.delta_time; float tilt_change = constrain_float(new_tilt - _angle_ef_target_rad.y,-max_change_rads,+max_change_rads); - _angle_ef_target_rad.y += tilt_change; + // Update tilt + _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y + tilt_change,_tilt_angle_min,_tilt_angle_max); }