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

AP_Mount: backend set_angle_target enforces bf limits

This commit is contained in:
Randy Mackay 2023-06-07 17:07:07 +09:00 committed by Andrew Tridgell
parent 514434193d
commit 39eb46fd26
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -30,6 +30,18 @@ bool AP_Mount_Backend::has_pitch_control() const
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
{
// enforce angle limits
roll_deg = constrain_float(roll_deg, _params.roll_angle_min, _params.roll_angle_max);
pitch_deg = constrain_float(pitch_deg, _params.pitch_angle_min, _params.pitch_angle_max);
if (!yaw_is_earth_frame) {
// only limit yaw if in body-frame. earth-frame yaw limiting is backend specific
// custom wrap code (instead of wrap_180) to better handle yaw of <= -180
if (yaw_deg > 180) {
yaw_deg -= 360;
}
yaw_deg = constrain_float(yaw_deg, _params.yaw_angle_min, _params.yaw_angle_max);
}
// set angle targets
mavt_target.target_type = MountTargetType::ANGLE;
mavt_target.angle_rad.roll = radians(roll_deg);