Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Thomas Watson 2023-10-11 15:31:03 -05:00 committed by Andrew Tridgell
parent 79b7852357
commit fa31a5e838
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp
View File

@ -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_gripper:
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
break; 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: default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false; 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_gripper:
case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
break; 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: default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false; return false;