From 07160e298557b627bcf7ed3e604651b56ba668b9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 25 Mar 2023 14:50:56 +1100 Subject: [PATCH] AP_Mount: move get_bf_yaw to be a method on the object it takes --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 2 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 20 +++++++++---------- libraries/AP_Mount/AP_Mount_Backend.h | 12 +++++------ libraries/AP_Mount/AP_Mount_SToRM32.cpp | 4 ++-- .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 2 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 2 +- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 4 ++-- 7 files changed, 23 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 37e4b40258..f58796204d 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -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)); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index ee87bcc51b..ff68878cc2 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -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; } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index cc928988fa..066f7dd7e4 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -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 diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 8d52409d7e..bdea46bf14 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -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); diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index c7013e49ac..73a2617c3a 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -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; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index e4fea5d7b4..840d19803e 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -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; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 7865ae1135..4575a69ebd 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -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();