Skip to content

Commit

Permalink
AP_ServoRelayEvents: allow mavlink command of rcin scaled functions
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
tpwrules authored and tridge committed Oct 16, 2023
1 parent 79b7852 commit fa31a5e
Showing 1 changed file with 16 additions and 0 deletions.
16 changes: 16 additions & 0 deletions libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit fa31a5e

Please sign in to comment.