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

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));
 }
 

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;
     }

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

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);
 

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;
 

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;

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();