Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Siyi implements body-frame yaw limits

This commit is contained in:
Randy Mackay 2023-06-09 08:53:30 +09:00 committed by Andrew Tridgell
parent 39eb46fd26
commit 65fd8a7120
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -598,8 +598,18 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_
const float pitch_err_rad = (pitch_rad - current_angle_transformed.y); const float pitch_err_rad = (pitch_rad - current_angle_transformed.y);
const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
// convert yaw angle to body-frame the use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100) // convert yaw angle to body-frame
const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad; float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad;
// enforce body-frame yaw angle limits. If beyond limits always use body-frame control
const float yaw_bf_min = radians(_params.yaw_angle_min);
const float yaw_bf_max = radians(_params.yaw_angle_max);
if (yaw_bf_rad < yaw_bf_min || yaw_bf_rad > yaw_bf_max) {
yaw_bf_rad = constrain_float(yaw_bf_rad, yaw_bf_min, yaw_bf_max);
yaw_is_ef = false;
}
// use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100)
const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z); const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z);
const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);