From c73beffe1bb8ffba46511299b673002d4baf394d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 13 Oct 2018 18:41:11 +1100 Subject: [PATCH] AP_Mount: factor out a rate_input_rad, const various functions --- libraries/AP_Mount/AP_Mount_Backend.cpp | 61 ++++++++++++++----------- libraries/AP_Mount/AP_Mount_Backend.h | 8 +++- 2 files changed, 40 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9f2516a7d9..09697d9c9b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -80,53 +80,60 @@ void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_ } } +void AP_Mount_Backend::rate_input_rad(float &out, const RC_Channel *ch, float min, float max) const +{ + if (ch == nullptr) { + return; + } + out += ch->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + out = constrain_float(out, radians(min*0.01f), radians(max*0.01f)); +} + // update_targets_from_rc - updates angle targets using input from receiver void AP_Mount_Backend::update_targets_from_rc() { -#define rc_ch(i) rc().channel(i-1) - - uint8_t roll_rc_in = _state._roll_rc_in; - uint8_t tilt_rc_in = _state._tilt_rc_in; - uint8_t pan_rc_in = _state._pan_rc_in; + const RC_Channel *roll_ch = rc().channel(_state._roll_rc_in - 1); + const RC_Channel *tilt_ch = rc().channel(_state._tilt_rc_in - 1); + const RC_Channel *pan_ch = rc().channel(_state._pan_rc_in - 1); // if joystick_speed is defined then pilot input defines a rate of change of the angle if (_frontend._joystick_speed) { - // allow pilot speed position input to come directly from an RC_Channel - if (roll_rc_in && rc_ch(roll_rc_in)) { - _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - _angle_ef_target_rad.x = constrain_float(_angle_ef_target_rad.x, radians(_state._roll_angle_min*0.01f), radians(_state._roll_angle_max*0.01f)); - } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y, radians(_state._tilt_angle_min*0.01f), radians(_state._tilt_angle_max*0.01f)); - } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - _angle_ef_target_rad.z = constrain_float(_angle_ef_target_rad.z, radians(_state._pan_angle_min*0.01f), radians(_state._pan_angle_max*0.01f)); - } - } else { // allow pilot position input to come directly from an RC_Channel - if (roll_rc_in && (rc_ch(roll_rc_in))) { - _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _state._roll_angle_min, _state._roll_angle_max); + rate_input_rad(_angle_ef_target_rad.x, + roll_ch, + _state._roll_angle_min, + _state._roll_angle_max); + rate_input_rad(_angle_ef_target_rad.y, + tilt_ch, + _state._tilt_angle_min, + _state._tilt_angle_max); + rate_input_rad(_angle_ef_target_rad.z, + pan_ch, + _state._pan_angle_min, + _state._pan_angle_max); + } else { + // allow pilot rate input to come directly from an RC_Channel + if (roll_ch) { + _angle_ef_target_rad.x = angle_input_rad(roll_ch, _state._roll_angle_min, _state._roll_angle_max); } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _state._tilt_angle_min, _state._tilt_angle_max); + if (tilt_ch) { + _angle_ef_target_rad.y = angle_input_rad(tilt_ch, _state._tilt_angle_min, _state._tilt_angle_max); } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _state._pan_angle_min, _state._pan_angle_max); + if (pan_ch) { + _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max); } } } // returns the angle (degrees*100) that the RC_Channel input is receiving -int32_t AP_Mount_Backend::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +int32_t AP_Mount_Backend::angle_input(const RC_Channel* rc, int16_t angle_min, int16_t angle_max) { return (rc->get_reverse() ? -1 : 1) * (rc->get_radio_in() - rc->get_radio_min()) * (int32_t)(angle_max - angle_min) / (rc->get_radio_max() - rc->get_radio_min()) + (rc->get_reverse() ? angle_max : angle_min); } // returns the angle (radians) that the RC_Channel input is receiving -float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max) { return radians(angle_input(rc, angle_min, angle_max)*0.01f); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index ff2285d6cc..e236d6b8af 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -83,8 +83,8 @@ protected: void update_targets_from_rc(); // angle_input, angle_input_rad - convert RC input into an earth-frame target angle - int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max); - float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max); + int32_t angle_input(const RC_Channel* rc, int16_t angle_min, int16_t angle_max); + float angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max); // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan = true); @@ -96,4 +96,8 @@ protected: AP_Mount::mount_state &_state; // references to the parameters and state for this backend uint8_t _instance; // this instance's number Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and vehicle-relative pan angles in radians + +private: + + void rate_input_rad(float &out, const RC_Channel *ch, float min, float max) const; };