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

AP_Mount: Clarify angle frames of set_angle_target()

This commit is contained in:
Nick Exton 2024-05-16 16:57:25 +10:00 committed by Peter Barker
parent 562e5c0b72
commit 8b5539c055
4 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Mount/AP_Mount.cpp
View File

@ -263,6 +263,7 @@ void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock)
}
// set angle target in degrees
// roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void AP_Mount::set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
{

1
libraries/AP_Mount/AP_Mount.h
View File

@ -157,6 +157,7 @@ public:
void set_yaw_lock(uint8_t instance, bool yaw_lock);
// set angle target in degrees
// roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { set_angle_target(_primary, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); }
void set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);

13
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -67,6 +67,7 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
}
// set angle target in degrees
// roll and pitch are in earth-frame
// 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)
{
@ -277,8 +278,8 @@ void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packe
{
switch (get_mode()) {
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// input_a : Pitch in centi-degrees
// input_b : Roll in centi-degrees
// input_a : Pitch in centi-degrees (earth-frame)
// input_b : Roll in centi-degrees (earth-frame)
// input_c : Yaw in centi-degrees (interpreted as body-frame)
set_angle_target(packet.input_b * 0.01, packet.input_a * 0.01, packet.input_c * 0.01, false);
break;
@ -325,10 +326,10 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
return MAV_RESULT_ACCEPTED;
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
// set body-frame target angles (in degrees) from mavlink message
const float pitch_deg = packet.param1; // param1: pitch (in degrees)
const float roll_deg = packet.param2; // param2: roll in degrees
const float yaw_deg = packet.param3; // param3: yaw in degrees
// set target angles (in degrees) from mavlink message
const float pitch_deg = packet.param1; // param1: pitch (earth-frame, degrees)
const float roll_deg = packet.param2; // param2: roll (earth-frame, degrees)
const float yaw_deg = packet.param3; // param3: yaw (body-frame, degrees)
// warn if angles are invalid to catch angles sent in centi-degrees
if ((fabsf(pitch_deg) > 90) || (fabsf(roll_deg) > 180) || (fabsf(yaw_deg) > 360)) {

1
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -84,6 +84,7 @@ public:
void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
// set angle target in degrees
// roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);