From fa31a5e8385e686a695725d1cd17151a6b9a5f9b Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Wed, 11 Oct 2023 15:31:03 -0500 Subject: [PATCH] AP_ServoRelayEvents: allow mavlink command of rcin scaled functions Allow `MAV_CMD_DO_SET_SERVO` and `MAV_CMD_DO_REPEAT_SERVO` to be used on a servo output set to an RCINnScaled function (i.e. k_rcinN_mapped). Scaling is applied so that a commanded servo PWM of <=1000 maps to SERVOn_MIN, a PWM of 1500 maps to SERVOn_TRIM, and a PWM of >=2000 maps to SERVOn_MAX. Linear interpolation is performed between ranges. --- .../AP_ServoRelayEvents/AP_ServoRelayEvents.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp index 849d7376bb..a840a8bd64 100644 --- a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp +++ b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp @@ -44,6 +44,14 @@ bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm) case SRV_Channel::k_gripper: case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru break; + case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: { + // mapped channels are set up with a -/+ 4500 angle range by + // SRV_Channel::aux_servo_function_setup + int16_t angle_scaled = constrain_uint16(pwm, 1000, 2000); + angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500 + pwm = c->pwm_from_scaled_value(angle_scaled); + break; + } default: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel); return false; @@ -101,6 +109,14 @@ bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_valu case SRV_Channel::k_gripper: case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru break; + case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: { + // mapped channels are set up with a -/+ 4500 angle range by + // SRV_Channel::aux_servo_function_setup + int16_t angle_scaled = constrain_uint16(_servo_value, 1000, 2000); + angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500 + _servo_value = c->pwm_from_scaled_value(angle_scaled); + break; + } default: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel); return false;