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

AP_Mount: Added lead filter

Browse Source
This commit is contained in:
Jonathan Challinger 2014-08-26 05:52:04 -07:00 committed by Andrew Tridgell
parent 0738d75701
commit d03ed7a2c3
2 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_Mount/AP_Mount.cpp
View File

@ -213,6 +213,24 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0), AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0),
#endif #endif
// @Param: LEAD_RLL
// @DisplayName: Roll stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate, compensating for servo delay. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("LEAD_RLL", 17, AP_Mount, _roll_stb_lead, 0.0f),
// @Param: LEAD_PTCH
// @DisplayName: Pitch stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("LEAD_PTCH", 18, AP_Mount, _pitch_stb_lead, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -622,6 +640,21 @@ AP_Mount::stabilize()
if (_stab_tilt) { if (_stab_tilt) {
_tilt_angle -= degrees(_ahrs.pitch); _tilt_angle -= degrees(_ahrs.pitch);
} }
// Add lead filter.
const Vector3f &gyro = _ahrs.get_gyro();
if (_stab_roll && _roll_stb_lead != 0.0f && fabsf(_ahrs.pitch) < M_PI/3.0f) {
// Compute rate of change of euler roll angle
float roll_rate = gyro.x + (_ahrs.sin_pitch() / _ahrs.cos_pitch()) * (gyro.y * _ahrs.sin_roll() + gyro.z * _ahrs.cos_roll());
_roll_angle -= degrees(roll_rate) * _roll_stb_lead;
}
if (_stab_tilt && _pitch_stb_lead != 0.0f) {
// Compute rate of change of euler pitch angle
float pitch_rate = _ahrs.cos_pitch() * gyro.y - _ahrs.sin_roll() * gyro.z;
_tilt_angle -= degrees(pitch_rate) * _pitch_stb_lead;
}
} }
#endif #endif
} }

3
libraries/AP_Mount/AP_Mount.h
View File

@ -129,6 +129,9 @@ private:
AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan
AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan
AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = tilt, vector.z=pan AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = tilt, vector.z=pan
AP_Float _roll_stb_lead;
AP_Float _pitch_stb_lead;
}; };
#endif // __AP_MOUNT_H__ #endif // __AP_MOUNT_H__