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

AP_Mount: move get_bf_yaw to be a method on the object it takes

This commit is contained in:
Peter Barker 2023-03-25 14:50:56 +11:00 committed by Randy Mackay
parent 5f469f2596
commit 07160e2985
7 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -162,7 +162,7 @@ void AP_Mount_Alexmos::control_axis(const MountTarget& angle_target_rad)
outgoing_buffer.angle_speed.speed_pitch = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(degrees(angle_target_rad.pitch));
outgoing_buffer.angle_speed.speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(degrees(get_bf_yaw_angle(angle_target_rad)));
outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(angle_target_rad.get_bf_yaw());
send_command(CMD_CONTROL, (uint8_t *)&outgoing_buffer.angle_speed, sizeof(alexmos_angles_speed));
}

20
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -466,27 +466,27 @@ bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const
}
// return body-frame yaw angle from a mount target
float AP_Mount_Backend::get_bf_yaw_angle(const MountTarget& angle_rad) const
float AP_Mount_Backend::MountTarget::get_bf_yaw() const
{
if (angle_rad.yaw_is_ef) {
if (yaw_is_ef) {
// convert to body-frame
return wrap_PI(angle_rad.yaw - AP::ahrs().yaw);
return wrap_PI(yaw - AP::ahrs().yaw);
}
// target is already body-frame
return angle_rad.yaw;
return yaw;
}
// return earth-frame yaw angle from a mount target
float AP_Mount_Backend::get_ef_yaw_angle(const MountTarget& angle_rad) const
float AP_Mount_Backend::MountTarget::get_ef_yaw() const
{
if (angle_rad.yaw_is_ef) {
if (yaw_is_ef) {
// target is already earth-frame
return angle_rad.yaw;
return yaw;
}
// convert to earth-frame
return wrap_PI(angle_rad.yaw + AP::ahrs().yaw);
return wrap_PI(yaw + AP::ahrs().yaw);
}
// update angle targets using a given rate target
@ -501,9 +501,9 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
// ensure angle yaw frames matches rate yaw frame
if (angle_rad.yaw_is_ef != rate_rad.yaw_is_ef) {
if (rate_rad.yaw_is_ef) {
angle_rad.yaw = get_ef_yaw_angle(angle_rad);
angle_rad.yaw = angle_rad.get_ef_yaw();
} else {
angle_rad.yaw = get_bf_yaw_angle(angle_rad);
angle_rad.yaw = angle_rad.get_bf_yaw();
}
angle_rad.yaw_is_ef = rate_rad.yaw_is_ef;
}

12
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -161,6 +161,12 @@ protected:
float pitch;
float yaw;
bool yaw_is_ef;
// return body-frame yaw angle from a mount target (in radians)
float get_bf_yaw() const;
// return earth-frame yaw angle from a mount target (in radians)
float get_ef_yaw() const;
};
// returns true if user has configured a valid yaw angle range
@ -197,12 +203,6 @@ protected:
// returns true on success, false on failure
bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED;
// return body-frame yaw angle from a mount target
float get_bf_yaw_angle(const MountTarget& angle_rad) const;
// return earth-frame yaw angle from a mount target
float get_ef_yaw_angle(const MountTarget& angle_rad) const;
// update angle targets using a given rate target
// the resulting angle_rad yaw frame will match the rate_rad yaw frame
// assumes a 50hz update rate

4
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -102,7 +102,7 @@ void AP_Mount_SToRM32::update()
// get attitude as a quaternion. returns true on success
bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat)
{
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw());
return true;
}
@ -149,7 +149,7 @@ void AP_Mount_SToRM32::send_do_mount_control(const MountTarget& angle_target_rad
0, // confirmation of zero means this is the first time this message has been sent
-degrees(angle_target_rad.pitch),
degrees(angle_target_rad.roll),
-degrees(get_bf_yaw_angle(angle_target_rad)),
-degrees(angle_target_rad.get_bf_yaw()),
0, 0, 0, // param4 ~ param6 unused
MAV_MOUNT_MODE_MAVLINK_TARGETING);

2
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -171,7 +171,7 @@ void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target
// send CMD_SETANGLE (Note: reversed pitch and yaw)
cmd_set_angles_data.pitch = -degrees(angle_target_rad.pitch);
cmd_set_angles_data.roll = degrees(angle_target_rad.roll);
cmd_set_angles_data.yaw = -degrees(get_bf_yaw_angle(angle_target_rad));
cmd_set_angles_data.yaw = -degrees(angle_target_rad.get_bf_yaw());
uint8_t* buf = (uint8_t*)&cmd_set_angles_data;

2
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -139,7 +139,7 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
const AP_AHRS &ahrs = AP::ahrs();
// get target yaw in body-frame with limits applied
const float yaw_bf_rad = constrain_float(get_bf_yaw_angle(angle_rad), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
const float yaw_bf_rad = constrain_float(angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
// default output to target earth-frame roll and pitch angles, body-frame yaw
_angle_bf_output_rad.x = angle_rad.roll;

4
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -106,7 +106,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
if (!_gimbal.aligned()) {
return false;
}
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw());
return true;
}
@ -115,7 +115,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
*/
void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
{
_gimbal.update_target(Vector3f{_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad)});
_gimbal.update_target(Vector3f{_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw()});
_gimbal.receive_feedback(chan,msg);
AP_Logger *logger = AP_Logger::get_singleton();