Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SRV_Channel: fix RCInxScaled when input is range (instead of angle) #25538

Merged
merged 1 commit into from
Nov 15, 2023

Conversation

rmackay9
Copy link
Contributor

@rmackay9 rmackay9 commented Nov 14, 2023

This resolves a bug in the servo output when the SERVOx_FUNCTION = RCInXScaled and the RCInput is a range instead of an angle. For example on Copter the problem occurs when RCx_FUNCTION = RCIn3Scaled.. Details of the issue are explained in issue #25524 and is also discussed here.

In current master the PWM output will always be between SERVOx_TRIM (e.g. 1500) and SERVOx_MAX (e.g. 2000). After this change the PWM output is correctly in the SERVOx_MIN (e.g. 1000) to SERVOx_MAX (e.g. 2000) range.

I've tested this fairly carefully including before and after tests.

Test1 Fn=RCIN3Scaled RC3_TRIM=1106
RC3 input RC9 output (before) RC9 output (after)
1106 1500 1100
1500 1683 1482
1930 1900 1900
FS 1500 1500
     
Test2 Fn=RCIN3Scaled RC3_TRIM=1500
RC3 input RC9 output (before) RC9 output (after)
1106 1100 1100
1500 1500 1482
1930 1900 1900
FS 1500 1500
     
Test3 Fn=RCIN2Scaled RC2_TRIM=1520
RC2 input RC9 output RC9 output (after)
1093 1100 1100
1520 1500 1500
1936 1900 1900
FS 1500 1500
     
Test4 MP Servo/Relay Page  
Input RC9 output (before) RC9 output (after)
1000 1100 1100
1500 1500 1500
2000 1900 1900

I've also tested that the failsafe behaviour is unchanged. Namely in case of an RC failsafe, the PWM output stays unchanged OR moves to SERVOx_TRIM depending upon the SERVO_RC_FS_MSK parameter bits.

The above tests were done with these parameters

Param Value
RC3_MIN 1106
RC3_MAX 1930
RC3_TRIM 1106
RC3_DZ 30
RC2_MIN 1095
RC2_MAX 1934
RC2_TRIM 1520
RC2_DZ 20
SERVO9_MIN 1100
SERVO9_MAX 1900
SERVO9_TRIM 1500

@@ -60,7 +60,15 @@ void SRV_Channel::output_ch(void)
int16_t radio_in = c->get_radio_in();
if (passthrough_mapped) {
if (rc().has_valid_input()) {
radio_in = pwm_from_angle(c->norm_input_dz() * 4500);
switch (c->get_type()) {
case RC_Channel::ControlType::ANGLE:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I feel like this should also be the default rather thank falling through

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks very much for the feedback. I guess you mean that maybe we should move this new code up to line 60 where "radio_in" is first set? This crossed my mind as well but that would affect the simpler SERVOx_FUNCTION=RCINx (e.g. non-scaled) case which really does just simply pass through a PWM from input to output.

@tridge tridge merged commit 265f19b into ArduPilot:master Nov 15, 2023
86 checks passed
@rmackay9 rmackay9 deleted the rcinscaled-fix branch November 16, 2023 10:09
@IamPete1 IamPete1 removed the DevCallEU label Dec 6, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants