From cc39e36ee11846764bc50f28061e743586cf96cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 1 Feb 2021 13:26:31 -0300 Subject: [PATCH] AP_Mount: Add missing const in member functions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- libraries/AP_Mount/SoloGimbal.cpp | 8 ++++---- libraries/AP_Mount/SoloGimbal.h | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index 438f7ab471..bf3ef46a64 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -337,7 +337,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_roll_tilt(const Quaternion &quatEst) return gimbalRateDemVecTilt; } -Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst) +Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst) const { // quaternion demanded at the previous time step static float lastDemY; @@ -481,7 +481,7 @@ void SoloGimbal::write_logs() _log_del_vel.zero(); } -bool SoloGimbal::joints_near_limits() +bool SoloGimbal::joints_near_limits() const { return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135); } @@ -518,7 +518,7 @@ void SoloGimbal::_acal_save_calibrations() _gimbalParams.flash(); } -void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) +void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) const { float sin_theta = sinf(_measurement.joint_angles.y); float cos_theta = cosf(_measurement.joint_angles.y); @@ -533,7 +533,7 @@ void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f joint_rates.z = sec_phi*(ang_vel.z*cos_theta-ang_vel.x*sin_theta); } -void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) +void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) const { float sin_theta = sinf(_measurement.joint_angles.y); float cos_theta = cosf(_measurement.joint_angles.y); diff --git a/libraries/AP_Mount/SoloGimbal.h b/libraries/AP_Mount/SoloGimbal.h index 4b10285833..9041f793c7 100644 --- a/libraries/AP_Mount/SoloGimbal.h +++ b/libraries/AP_Mount/SoloGimbal.h @@ -66,7 +66,7 @@ public: void write_logs(); - float get_log_dt() { return _log_dt; } + float get_log_dt() const { return _log_dt; } void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); } void fetch_params() { _gimbalParams.fetch_params(); } @@ -84,12 +84,12 @@ private: Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst); Vector3f get_ang_vel_dem_roll_tilt(const Quaternion &quatEst); - Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst); + Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst) const; Vector3f get_ang_vel_dem_gyro_bias(); Vector3f get_ang_vel_dem_body_lock(); - void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates); - void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel); + void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) const; + void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) const; void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng); @@ -100,7 +100,7 @@ private: gimbal_mode_t get_mode(); - bool joints_near_limits(); + bool joints_near_limits() const; // private member variables SoloGimbalEKF _ekf; // state of small EKF for gimbal