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

AP_Mount: do not allow both attitude and rate for GIMBAL_MANAGER_SET_ATTITUDE 

this will allow us to support both at the same time into the future without worrying about how it might break existing callers.
This commit is contained in:
Peter Barker 2023-03-25 12:30:22 +11:00 committed by Peter Barker
parent 91935fc404
commit c63ec307f1
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Mount/AP_Mount.cpp
View File

@ -370,6 +370,19 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
}
const Quaternion att_quat{packet.q};
const Vector3f att_rate_degs {
packet.angular_velocity_x,
packet.angular_velocity_y,
packet.angular_velocity_y
};
// ensure that we are only demanded to a specific attitude or to
// achieve a specific rate. Do not allow both to be specified at
// the same time:
if (!att_quat.is_nan() && !att_rate_degs.is_nan()) {
return;
}
if (!att_quat.is_nan()) {
// convert quaternion to euler angles
float roll_rad, pitch_rad, yaw_rad;
@ -383,7 +396,7 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
return;
}
if (!isnan(packet.angular_velocity_x) && !isnan(packet.angular_velocity_y) && !isnan(packet.angular_velocity_y)) {
{
const float roll_rate_degs = degrees(packet.angular_velocity_x);
const float pitch_rate_degs = degrees(packet.angular_velocity_y);
const float yaw_rate_degs = degrees(packet.angular_velocity_z);