From 8092697c1a5dcd2750b15440ee48142933ffc261 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Jun 2022 16:03:41 +0900 Subject: [PATCH] AP_Mount: backend record RC rate targets --- libraries/AP_Mount/AP_Mount_Backend.cpp | 32 ++++++++++++------------- libraries/AP_Mount/AP_Mount_Backend.h | 7 +++++- 2 files changed, 22 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index dec5d0fc1c..271dccf8aa 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -4,6 +4,8 @@ extern const AP_HAL::HAL& hal; +#define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate + // set_angle_targets - sets angle targets in degrees void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) { @@ -114,13 +116,17 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli return true; } -void AP_Mount_Backend::rate_input_rad(float &out, const RC_Channel *chan, float min, float max) const +// 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 { if ((chan == nullptr) || (chan->get_radio_in() == 0)) { + rate_rads = 0; return; } - out += chan->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - out = constrain_float(out, radians(min*0.01f), radians(max*0.01f)); + rate_rads = chan->norm_input_dz() * 0.005f * _frontend._joystick_speed; + angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_rad*0.01f), radians(angle_max_rad*0.01f)); } // update_targets_from_rc - updates angle targets using input from receiver @@ -131,20 +137,12 @@ void AP_Mount_Backend::update_targets_from_rc() 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) { + if (_frontend._joystick_speed > 0) { // allow pilot position input to come directly from an RC_Channel - 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); + update_rate_and_angle_from_rc(roll_ch, _rate_target_rads.x, _angle_ef_target_rad.x, _state._roll_angle_min, _state._roll_angle_max); + update_rate_and_angle_from_rc(tilt_ch, _rate_target_rads.y, _angle_ef_target_rad.y, _state._tilt_angle_min, _state._tilt_angle_max); + update_rate_and_angle_from_rc(pan_ch, _rate_target_rads.z, _angle_ef_target_rad.z, _state._pan_angle_min, _state._pan_angle_max); + _rate_target_rads_valid = true; } else { // allow pilot rate input to come directly from an RC_Channel if ((roll_ch != nullptr) && (roll_ch->get_radio_in() != 0)) { @@ -156,6 +154,8 @@ void AP_Mount_Backend::update_targets_from_rc() if ((pan_ch != nullptr) && (pan_ch->get_radio_in() != 0)) { _angle_ef_target_rad.z = angle_input_rad(pan_ch, _state._pan_angle_min, _state._pan_angle_max); } + // not using rate input + _rate_target_rads_valid = false; } } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 87f63fc5f1..a51500d605 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -118,10 +118,15 @@ 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 + Vector3f _rate_target_rads; // desired roll, pitch, yaw rate in radians/sec + bool _rate_target_rads_valid;// true if _rate_target_rads should can be used (e.g. RC input is using rate control) private: - void rate_input_rad(float &out, const RC_Channel *ch, float min, float max) const; + // 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; }; #endif // HAL_MOUNT_ENABLED