From f28f5a1ad5af7c43ea255512f94be18b7dfab3fb Mon Sep 17 00:00:00 2001 From: TunaLobster Date: Fri, 5 Aug 2022 15:07:04 -0500 Subject: [PATCH] SRV_Channel: Add SRV fucntions for scaled RC passthrough --- libraries/SRV_Channel/SRV_Channel.cpp | 8 ++++---- libraries/SRV_Channel/SRV_Channel.h | 16 ++++++++++++++++ libraries/SRV_Channel/SRV_Channel_aux.cpp | 12 +++++++++++- 3 files changed, 31 insertions(+), 5 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 38269c2e56..11c1f36129 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -64,10 +64,10 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Param: FUNCTION // @DisplayName: Servo output function // @Description: Function assigned to this servo. Setting this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function - // @Values: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,71:TrackerYaw,72:TrackerPitch,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted - // @Values{Plane}: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7/TailTiltServo,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,110:Airbrakes,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted - // @Values{Copter}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,88:Winch,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted - // @Values{Rover}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,26:GroundSteering,28:Gripper,33:Motor1,34:Motor2,35:Motor3,36:Motor4,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,70:Throttle,73:ThrottleLeft,74:ThrottleRight,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted + // @Values: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,71:TrackerYaw,72:TrackerPitch,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled,156:RCIN17Scaled + // @Values{Plane}: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7/TailTiltServo,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,110:Airbrakes,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled,156:RCIN17Scaled + // @Values{Copter}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,88:Winch,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled,156:RCIN17Scaled + // @Values{Rover}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,26:GroundSteering,28:Gripper,33:Motor1,34:Motor2,35:Motor3,36:Motor4,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,70:Throttle,73:ThrottleLeft,74:ThrottleRight,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled,156:RCIN17Scaled // @User: Standard // @RebootRequired: True AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 7994cbbc2b..341b642af5 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -183,6 +183,22 @@ public: k_mast_rotation = 137, k_alarm = 138, k_alarm_inverted = 139, + k_rcin1_mapped = 140, + k_rcin2_mapped = 141, + k_rcin3_mapped = 142, + k_rcin4_mapped = 143, + k_rcin5_mapped = 144, + k_rcin6_mapped = 145, + k_rcin7_mapped = 146, + k_rcin8_mapped = 147, + k_rcin9_mapped = 148, + k_rcin10_mapped = 149, + k_rcin11_mapped = 150, + k_rcin12_mapped = 151, + k_rcin13_mapped = 152, + k_rcin14_mapped = 153, + k_rcin15_mapped = 154, + k_rcin16_mapped = 155, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 4a1f587534..943b79519b 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -32,6 +32,7 @@ void SRV_Channel::output_ch(void) { #ifndef HAL_BUILD_AP_PERIPH int8_t passthrough_from = -1; + bool passthrough_mapped = false; // take care of special function cases switch(function.get()) @@ -42,6 +43,10 @@ void SRV_Channel::output_ch(void) case k_rcin1 ... k_rcin16: // rc pass-thru passthrough_from = int8_t((int16_t)function - k_rcin1); break; + case k_rcin1_mapped ... k_rcin16_mapped: + passthrough_from = int8_t((int16_t)function - k_rcin1_mapped); + passthrough_mapped = true; + break; } if (passthrough_from != -1) { // we are doing passthrough from input to output for this channel @@ -50,7 +55,11 @@ void SRV_Channel::output_ch(void) if (SRV_Channels::passthrough_disabled()) { output_pwm = c->get_radio_trim(); } else { - const int16_t radio_in = c->get_radio_in(); + // non-mapped rc passthrough + int16_t radio_in = c->get_radio_in(); + if (passthrough_mapped) { + radio_in = pwm_from_angle(c->norm_input_dz() * 4500); + } if (!ign_small_rcin_changes) { output_pwm = radio_in; previous_radio_in = radio_in; @@ -159,6 +168,7 @@ void SRV_Channel::aux_servo_function_setup(void) case k_roll_out: case k_pitch_out: case k_yaw_out: + case k_rcin1_mapped ... k_rcin16_mapped: set_angle(4500); break; case k_throttle: