From 3711c362ce78c1082f77e0dd26384d41631a4078 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 17 Jun 2022 15:07:14 +0900 Subject: [PATCH] AP_Mount: update_rate_and_angle_from_rc fix arg name to match units --- libraries/AP_Mount/AP_Mount_Backend.cpp | 5 +++-- libraries/AP_Mount/AP_Mount_Backend.h | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index a5d0c4844e..f72c7f7adc 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -119,14 +119,15 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli // update rate and angle targets from RC input // current angle target (in radians) should be provided in angle_rad target // rate and angle targets are returned in rate_rads and angle_rad arguments -void AP_Mount_Backend::update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_rad, float angle_max_rad) const +// angle min and max are in centi-degrees +void AP_Mount_Backend::update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_cd, float angle_max_cd) const { if ((chan == nullptr) || (chan->get_radio_in() == 0)) { rate_rads = 0; return; } rate_rads = chan->norm_input_dz() * radians(_frontend._rc_rate_max); - angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_rad*0.01f), radians(angle_max_rad*0.01f)); + angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_cd*0.01f), radians(angle_max_cd*0.01f)); } // update_targets_from_rc - updates angle targets using input from receiver diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index d46a3bcea8..5fb3dbdfdd 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -135,7 +135,8 @@ private: // update rate and angle targets from RC input // current angle target (in radians) should be provided in angle_rad target // rate and angle targets are returned in rate_rads and angle_rad arguments - void update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_rad, float angle_max_rad) const; + // angle min and max are in centi-degrees + void update_rate_and_angle_from_rc(const RC_Channel *chan, float &rate_rads, float &angle_rad, float angle_min_cd, float angle_max_cd) const; }; #endif // HAL_MOUNT_ENABLED