Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : SRV_Channel.cpp - object to separate input and output channel
17 : ranges, trim and reversal
18 : */
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_Math/AP_Math.h>
22 : #include <AP_Vehicle/AP_Vehicle_Type.h>
23 : #include "SRV_Channel.h"
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : SRV_Channel::servo_mask_t SRV_Channel::have_pwm_mask;
28 :
29 : const AP_Param::GroupInfo SRV_Channel::var_info[] = {
30 : // @Param: MIN
31 : // @DisplayName: Minimum PWM
32 : // @Description: minimum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
33 : // @Units: PWM
34 : // @Range: 800 2200
35 : // @Increment: 1
36 : // @User: Standard
37 : AP_GROUPINFO("MIN", 1, SRV_Channel, servo_min, 1100),
38 :
39 : // @Param: MAX
40 : // @DisplayName: Maximum PWM
41 : // @Description: maximum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
42 : // @Units: PWM
43 : // @Range: 800 2200
44 : // @Increment: 1
45 : // @User: Standard
46 : AP_GROUPINFO("MAX", 2, SRV_Channel, servo_max, 1900),
47 :
48 : // @Param: TRIM
49 : // @DisplayName: Trim PWM
50 : // @Description: Trim PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
51 : // @Units: PWM
52 : // @Range: 800 2200
53 : // @Increment: 1
54 : // @User: Standard
55 : AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500),
56 :
57 : // @Param: REVERSED
58 : // @DisplayName: Servo reverse
59 : // @Description: Reverse servo operation. Set to 0 for normal operation. Set to 1 to reverse this output channel.
60 : // @Values: 0:Normal,1:Reversed
61 : // @User: Standard
62 : AP_GROUPINFO("REVERSED", 4, SRV_Channel, reversed, 0),
63 :
64 : // @Param: FUNCTION
65 : // @DisplayName: Servo output function
66 : // @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
67 : // @SortValues: AlphabeticalZeroAtTop
68 : // @Values: -1:GPIO
69 : // @Values{Plane, Copter, Rover, Sub}: -1:GPIO
70 : // @Values: 0:Disabled
71 : // @Values{Plane, Copter, Rover, Sub}: 0:Disabled
72 : // @Values: 1:RCPassThru
73 : // @Values{Plane, Copter, Rover, Sub}: 1:RCPassThru
74 : // @Values: 2:Flap, 3:FlapAuto
75 : // @Values{Plane}: 2:Flap,3:FlapAuto
76 : // @Values: 4:Aileron
77 : // @Values{Plane}: 4:Aileron
78 : // @Values: 6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract
79 : // @Values{Plane, Copter, Rover, Sub}: 6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract
80 : // @Values: 10:CameraTrigger
81 : // @Values{Plane, Copter, Rover}: 10:CameraTrigger
82 : // @Values: 12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract
83 : // @Values{Plane, Copter, Rover}: 12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract
84 : // @Values: 16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1
85 : // @Values{Plane}: 16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1
86 : // @Values: 19:Elevator
87 : // @Values{Plane}: 19:Elevator
88 : // @Values: 21:Rudder
89 : // @Values{Plane}: 21:Rudder
90 : // @Values: 22:SprayerPump,23:SprayerSpinner
91 : // @Values{Plane, Copter, Rover}: 22:SprayerPump,23:SprayerSpinner
92 : // @Values: 24:FlaperonLeft,25:FlaperonRight
93 : // @Values{Plane}: 24:FlaperonLeft,25:FlaperonRight
94 : // @Values: 26:GroundSteering
95 : // @Values{Plane, Rover}: 26:GroundSteering
96 : // @Values: 27:Parachute
97 : // @Values{Plane, Copter}: 27:Parachute
98 : // @Values: 28:Gripper
99 : // @Values{Plane, Copter, Rover, Sub}: 28:Gripper
100 : // @Values: 29:LandingGear
101 : // @Values{Plane, Copter}: 29:LandingGear
102 : // @Values: 30:EngineRunEnable
103 : // @Values{Plane, Copter}: 30:EngineRunEnable
104 : // @Values: 31:HeliRSC,32:HeliTailRSC
105 : // @Values{Copter}: 31:HeliRSC,32:HeliTailRSC
106 : // @Values: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8
107 : // @Values{Plane}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7/TailTiltServo,40:Motor8
108 : // @Values{Copter, Sub}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8
109 : // @Values{Rover}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4
110 : // @Values: 41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight
111 : // @Values{Plane}: 41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight
112 : // @Values: 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
113 : // @Values{Plane, Copter, Rover}: 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
114 : // @Values{Sub}: 51:RCIN1/Pitch,52:RCIN2/Roll,53:RCIN3/HeaveVertical,54:RCIN4/YawTurn,55:RCIN5/SurgeForward,56:RCIN6/SwayLateral,57:RCIN7/CameraPan,58:RCIN8/CameraTilt,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16
115 : // @Values: 67:Ignition,69:Starter
116 : // @Values{Plane}: 67:Ignition,69:Starter
117 : // @Values: 70:Throttle
118 : // @Values{Plane, Rover}: 70:Throttle
119 : // @Values: 71:TrackerYaw,72:TrackerPitch
120 : // @Values: 73:ThrottleLeft,74:ThrottleRight
121 : // @Values{Plane, Copter, Rover}: 73:ThrottleLeft,74:ThrottleRight
122 : // @Values: 75:TiltMotorFrontLeft,76:TiltMotorFrontRight
123 : // @Values{Plane, Copter}: 75:TiltMotorFrontLeft,76:TiltMotorFrontRight
124 : // @Values: 77:ElevonLeft,78:ElevonRight
125 : // @Values{Plane}: 77:ElevonLeft,78:ElevonRight
126 : // @Values: 79:VTailLeft,80:VTailRight
127 : // @Values{Plane}: 79:VTailLeft,80:VTailRight
128 : // @Values: 81:BoostThrottle
129 : // @Values{Copter}: 81:BoostThrottle
130 : // @Values: 82:Motor9,83:Motor10,84:Motor11,85:Motor12
131 : // @Values{Plane, Copter, Sub}: 82:Motor9,83:Motor10,84:Motor11,85:Motor12
132 : // @Values: 86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2
133 : // @Values{Plane}: 86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2
134 : // @Values: 88:Winch
135 : // @Values{Copter, Rover, Sub}: 88:Winch
136 : // @Values: 89:Main Sail
137 : // @Values{Rover}: 89:Main Sail
138 : // @Values: 90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed
139 : // @Values{Plane, Copter, Rover, Sub}: 90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed
140 : // @Values: 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
141 : // @Values{Plane, Copter, Rover, Sub}: 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
142 : // @Values{Plane}: 110:Airbrakes
143 : // @Values: 120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4
144 : // @Values{Plane, Copter, Rover, Sub}: 120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4
145 : // @Values: 124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw
146 : // @Values{Plane, Copter}: 124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw
147 : // @Values: 128:WingSailElevator
148 : // @Values{Rover}: 128:WingSailElevator
149 : // @Values: 129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock
150 : // @Values{Plane, Copter, Rover}: 129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock
151 : // @Values: 133:Winch Clutch
152 : // @Values{Copter, Rover}: 133:Winch Clutch
153 : // @Values: 134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX
154 : // @Values{Plane, Copter, Rover, Sub}: 134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX
155 : // @Values: 137:SailMastRotation
156 : // @Values{Rover}: 137:SailMastRotation
157 : // @Values: 138:Alarm,139:Alarm Inverted
158 : // @Values{Plane, Copter, Rover}: 138:Alarm,139:Alarm Inverted
159 : // @Values: 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
160 : // @Values{Plane, Copter, Rover}: 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
161 : // @Values{Plane, Copter}: 160:Motor13,161:Motor14,162:Motor15,163:Motor16
162 : // @Values{Plane, Copter}: 164:Motor17,165:Motor18,166:Motor19,167:Motor20
163 : // @Values{Plane, Copter}: 168:Motor21,169:Motor22,170:Motor23,171:Motor24
164 : // @Values{Plane, Copter}: 172:Motor25,173:Motor26,174:Motor27,175:Motor28
165 : // @Values{Plane, Copter}: 176:Motor29,177:Motor30,178:Motor31,179:Motor32
166 : // @Values: 180:CameraZoom
167 : // @Values{Sub}: 181:Lights1,182:Lights2
168 : // @Values{Sub}: 183:VideoSwitch
169 : // @Values{Sub}: 184:Actuator1,185:Actuator2,186:Actuator3,187:Actuator4,188:Actuator5,189:Actuator6
170 : // @User: Standard
171 : // @RebootRequired: True
172 : AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0),
173 :
174 : AP_GROUPEND
175 : };
176 :
177 :
178 67008 : SRV_Channel::SRV_Channel(void)
179 : {
180 67008 : AP_Param::setup_object_defaults(this, var_info);
181 : // start with all pwm at zero
182 67008 : have_pwm_mask = ~uint32_t(0);
183 67008 : }
184 :
185 : // convert a 0..range_max to a pwm
186 15332303 : uint16_t SRV_Channel::pwm_from_range(float scaled_value) const
187 : {
188 15332303 : if (servo_max <= servo_min || high_out == 0) {
189 0 : return servo_min;
190 : }
191 15332303 : scaled_value = constrain_float(scaled_value, 0, high_out);
192 15332303 : if (reversed) {
193 299118 : scaled_value = high_out - scaled_value;
194 : }
195 15332303 : return servo_min + uint16_t( (scaled_value * (float)(servo_max - servo_min)) / (float)high_out );
196 : }
197 :
198 : // convert a -angle_max..angle_max to a pwm
199 31064465 : uint16_t SRV_Channel::pwm_from_angle(float scaled_value) const
200 : {
201 31064465 : if (high_out == 0) {
202 426 : return servo_trim;
203 : }
204 31064039 : if (reversed) {
205 3537952 : scaled_value = -scaled_value;
206 : }
207 31064039 : scaled_value = constrain_float(scaled_value, -high_out, high_out);
208 31064039 : if (scaled_value > 0) {
209 13293276 : return servo_trim + uint16_t( (scaled_value * (float)(servo_max - servo_trim)) / (float)high_out);
210 : } else {
211 17770763 : return servo_trim - uint16_t( (-scaled_value * (float)(servo_trim - servo_min)) / (float)high_out);
212 : }
213 : }
214 :
215 46386012 : uint16_t SRV_Channel::pwm_from_scaled_value(float scaled_value) const
216 : {
217 46386012 : if (type_angle) {
218 31053709 : return pwm_from_angle(scaled_value);
219 : } else {
220 15332303 : return pwm_from_range(scaled_value);
221 : }
222 : }
223 :
224 671375967 : void SRV_Channel::calc_pwm(float output_scaled)
225 : {
226 671375967 : if (have_pwm_mask & (1U<<ch_num)) {
227 : // Note that this allows a set_output_pwm call to override E-Stop!!
228 : // tricky to fix because we would endup E-stoping to individual SEROVx_MIN not MOT_PWM_MIN on copter
229 624989955 : return;
230 : }
231 :
232 : // check for E - stop
233 46386012 : bool force = false;
234 46386012 : if (SRV_Channel::should_e_stop(get_function()) && SRV_Channels::emergency_stop) {
235 1160 : output_scaled = 0.0;
236 1160 : force = true;
237 : }
238 :
239 46386012 : if (!force && override_active) {
240 : // don't overwrite a override
241 0 : return;
242 : }
243 :
244 46386012 : output_pwm = pwm_from_scaled_value(output_scaled);
245 : }
246 :
247 166042225 : void SRV_Channel::set_output_pwm(uint16_t pwm, bool force)
248 : {
249 166042225 : if (!override_active || force) {
250 166042225 : output_pwm = pwm;
251 166042225 : have_pwm_mask |= (1U<<ch_num);
252 : }
253 166042225 : }
254 :
255 : // set normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
256 0 : void SRV_Channel::set_output_norm(float value)
257 : {
258 : // convert normalised value to pwm
259 0 : set_output_pwm(pwm_from_scaled_value(value * high_out));
260 0 : }
261 :
262 : // set angular range of scaled output
263 435285 : void SRV_Channel::set_angle(int16_t angle)
264 : {
265 435285 : type_angle = true;
266 435285 : high_out = angle;
267 435285 : type_setup = true;
268 435285 : }
269 :
270 : // set range of scaled output
271 2580576 : void SRV_Channel::set_range(uint16_t high)
272 : {
273 2580576 : type_angle = false;
274 2580576 : high_out = high;
275 2580576 : type_setup = true;
276 2580576 : }
277 :
278 : /*
279 : get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
280 : */
281 104138 : float SRV_Channel::get_output_norm(void)
282 : {
283 104138 : uint16_t mid = (servo_max + servo_min) / 2;
284 : float ret;
285 104138 : if (mid <= servo_min) {
286 0 : return 0;
287 : }
288 104138 : if (output_pwm < mid) {
289 52141 : ret = (float)(output_pwm - mid) / (float)(mid - servo_min);
290 51997 : } else if (output_pwm > mid) {
291 723 : ret = (float)(output_pwm - mid) / (float)(servo_max - mid);
292 : } else {
293 51274 : ret = 0;
294 : }
295 104138 : if (get_reversed()) {
296 0 : ret = -ret;
297 : }
298 104138 : return ret;
299 : }
300 :
301 436907 : uint16_t SRV_Channel::get_limit_pwm(Limit limit) const
302 : {
303 436907 : switch (limit) {
304 427347 : case Limit::TRIM:
305 427347 : return servo_trim;
306 665 : case Limit::MIN:
307 665 : return reversed?servo_max:servo_min;
308 18 : case Limit::MAX:
309 18 : return reversed?servo_min:servo_max;
310 8877 : case Limit::ZERO_PWM:
311 8877 : return 0;
312 : }
313 0 : return 0;
314 : }
315 :
316 : // return true if function is for a multicopter motor
317 22112 : bool SRV_Channel::is_motor(SRV_Channel::Function function)
318 : {
319 22112 : return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) ||
320 44224 : (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12) ||
321 22112 : (function >= SRV_Channel::k_motor13 && function <= SRV_Channel::k_motor32));
322 : }
323 :
324 : // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
325 46386012 : bool SRV_Channel::should_e_stop(SRV_Channel::Function function)
326 : {
327 46386012 : switch (function) {
328 16459368 : case Function::k_heli_rsc:
329 : case Function::k_heli_tail_rsc:
330 : case Function::k_motor1:
331 : case Function::k_motor2:
332 : case Function::k_motor3:
333 : case Function::k_motor4:
334 : case Function::k_motor5:
335 : case Function::k_motor6:
336 : case Function::k_motor7:
337 : case Function::k_motor8:
338 : case Function::k_starter:
339 : case Function::k_throttle:
340 : case Function::k_throttleLeft:
341 : case Function::k_throttleRight:
342 : case Function::k_boost_throttle:
343 : case Function::k_motor9:
344 : case Function::k_motor10:
345 : case Function::k_motor11:
346 : case Function::k_motor12:
347 : case Function::k_motor13 ... Function::k_motor32:
348 : case Function::k_engine_run_enable:
349 16459368 : return true;
350 29926644 : default:
351 29926644 : return false;
352 : }
353 : return false;
354 : }
355 :
356 : // return true if function is for a control surface
357 0 : bool SRV_Channel::is_control_surface(SRV_Channel::Function function)
358 : {
359 0 : switch (function) {
360 0 : case Function::k_flap:
361 : case Function::k_flap_auto:
362 : case Function::k_aileron:
363 : case Function::k_dspoilerLeft1:
364 : case Function::k_dspoilerLeft2:
365 : case Function::k_dspoilerRight1:
366 : case Function::k_dspoilerRight2:
367 : case Function::k_elevator:
368 : case Function::k_rudder:
369 : case Function::k_flaperon_left:
370 : case Function::k_flaperon_right:
371 : case Function::k_elevon_left:
372 : case Function::k_elevon_right:
373 : case Function::k_vtail_left:
374 : case Function::k_vtail_right:
375 : case Function::k_airbrake:
376 0 : return true;
377 :
378 0 : default:
379 0 : return false;
380 : }
381 :
382 : return false;
383 : }
384 :
385 : // return the motor number of a channel, or -1 if not a motor
386 : // return 0 for first motor
387 0 : int8_t SRV_Channel::get_motor_num(void) const
388 : {
389 0 : const auto k_function = get_function();
390 0 : switch (k_function) {
391 0 : case k_motor1 ... k_motor8:
392 0 : return int8_t(uint16_t(k_function) - k_motor1);
393 0 : case k_motor9 ... k_motor12:
394 0 : return 8 + int8_t(uint16_t(k_function) - k_motor9);
395 0 : case k_motor13 ... k_motor32:
396 0 : return 12 + int8_t(uint16_t(k_function) - k_motor13);
397 0 : default:
398 0 : break;
399 : }
400 0 : return -1;
401 : }
|
