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 : #include <stdlib.h>
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <SRV_Channel/SRV_Channel.h>
19 : #include <AP_Logger/AP_Logger.h>
20 :
21 : #include "AP_MotorsHeli_Swash.h"
22 :
23 : extern const AP_HAL::HAL& hal;
24 :
25 : const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
26 :
27 : // @Param: TYPE
28 : // @DisplayName: Swashplate Type
29 : // @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
30 : // @Values: 0:H3 Generic,1:H1 non-CPPM,2:H3_140,3:H3_120,4:H4_90,5:H4_45
31 : // @User: Standard
32 : AP_GROUPINFO("TYPE", 1, AP_MotorsHeli_Swash, _swashplate_type, SWASHPLATE_TYPE_H3_120),
33 :
34 : // @Param: COL_DIR
35 : // @DisplayName: Swashplate Collective Control Direction
36 : // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
37 : // @Values: 0:Normal,1:Reversed
38 : // @User: Standard
39 : AP_GROUPINFO("COL_DIR", 2, AP_MotorsHeli_Swash, _swash_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
40 :
41 : // @Param: LIN_SVO
42 : // @DisplayName: Linearize Swashplate Servo Mechanical Throw
43 : // @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
44 : // @Values: 0:Disabled,1:Enabled
45 : // @User: Standard
46 : AP_GROUPINFO("LIN_SVO", 3, AP_MotorsHeli_Swash, _linear_swash_servo, 0),
47 :
48 : // @Param: PHANG
49 : // @DisplayName: Swashplate Phase Angle Compensation
50 : // @Description: Phase angle compensation can be used to correct control coupling issues. If pitching the swash forward induces a roll, this can be correct the problem.
51 : // @Range: -30 30
52 : // @Units: deg
53 : // @User: Advanced
54 : // @Increment: 1
55 : AP_GROUPINFO("PHANG", 9, AP_MotorsHeli_Swash, _phase_angle, 0),
56 :
57 : // @Param: H3_ENABLE
58 : // @DisplayName: Enable Generic H3 Swashplate Settings
59 : // @Description: Automatically set when H3 generic swash type is selected. Do not set manually.
60 : // @Values: 0:Disabled,1:Enabled
61 : // @User: Advanced
62 : AP_GROUPINFO_FLAGS("H3_ENABLE", 4, AP_MotorsHeli_Swash, enable, 0, AP_PARAM_FLAG_ENABLE),
63 :
64 : // @Param: H3_SV1_POS
65 : // @DisplayName: Swashplate Servo 1 Position
66 : // @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
67 : // @Range: -180 180
68 : // @Units: deg
69 : // @User: Advanced
70 : AP_GROUPINFO("H3_SV1_POS", 5, AP_MotorsHeli_Swash, _servo1_pos, -60),
71 :
72 : // @Param: H3_SV2_POS
73 : // @DisplayName: Swashplate Servo 2 Position
74 : // @Description: Azimuth position on swashplate for servo 2 with the front of the heli being 0 deg
75 : // @Range: -180 180
76 : // @Units: deg
77 : // @User: Advanced
78 : AP_GROUPINFO("H3_SV2_POS", 6, AP_MotorsHeli_Swash, _servo2_pos, 60),
79 :
80 : // @Param: H3_SV3_POS
81 : // @DisplayName: Swashplate Servo 3 Position
82 : // @Description: Azimuth position on swashplate for servo 3 with the front of the heli being 0 deg
83 : // @Range: -180 180
84 : // @Units: deg
85 : // @User: Advanced
86 : AP_GROUPINFO("H3_SV3_POS", 7, AP_MotorsHeli_Swash, _servo3_pos, 180),
87 :
88 : // index 8 was previously used for H3 phase angle, but this has been moved to index 9 to allow the use for all swashplate types.
89 :
90 : AP_GROUPEND
91 : };
92 :
93 80 : AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t instance) :
94 80 : _motor_num{mot_0, mot_1, mot_2, mot_3},
95 80 : _instance(instance)
96 : {
97 80 : AP_Param::setup_object_defaults(this, var_info);
98 80 : }
99 :
100 : // configure - configure the swashplate settings for any updated parameters
101 859845 : void AP_MotorsHeli_Swash::configure()
102 : {
103 :
104 859845 : _swash_type = static_cast<SwashPlateType>(_swashplate_type.get());
105 859845 : _collective_direction = static_cast<CollectiveDirection>(_swash_coll_dir.get());
106 859845 : _make_servo_linear = _linear_swash_servo != 0;
107 859845 : enable.set(_swash_type == SWASHPLATE_TYPE_H3);
108 :
109 859845 : calculate_roll_pitch_collective_factors();
110 859845 : }
111 :
112 : // CCPM Mixers - calculate mixing scale factors by swashplate type
113 859845 : void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
114 : {
115 : // Clear existing setup
116 4299225 : for (uint8_t i = 0; i < _max_num_servos; i++) {
117 3439380 : _enabled[i] = false;
118 3439380 : _rollFactor[i] = 0.0;
119 3439380 : _pitchFactor[i] = 0.0;
120 3439380 : _collectiveFactor[i] = 0.0;
121 : }
122 :
123 859845 : switch (_swash_type) {
124 0 : case SWASHPLATE_TYPE_H3:
125 : {
126 : // Three-servo roll/pitch mixer for adjustable servo position
127 : // can be any style swashplate, phase angle is adjustable
128 0 : add_servo_angle(CH_1, 1.0, _servo1_pos, 1.0);
129 0 : add_servo_angle(CH_2, 1.0, _servo2_pos, 1.0);
130 0 : add_servo_angle(CH_3, 1.0, _servo3_pos, 1.0);
131 0 : break;
132 : }
133 0 : case SWASHPLATE_TYPE_H1:
134 : {
135 : // CCPM mixing not being used, so H1 straight outputs. phase angle is adjustable
136 0 : add_servo_angle(CH_1, 1.0, -90.0, 0.0);
137 0 : add_servo_angle(CH_2, 1.0, 0.0, 0.0);
138 0 : add_servo_raw(CH_3, 0.0, 0.0, 1.0);
139 0 : break;
140 : }
141 :
142 0 : case SWASHPLATE_TYPE_H3_140:
143 : {
144 : // Three-servo roll/pitch mixer for H3-140
145 : // HR3-140 uses reversed servo and collective direction in heli setup
146 : // 1:1 pure input style, phase angle is adjustable
147 0 : add_servo_angle(CH_1, safe_sqrt(2), -45.0, 1.0);
148 0 : add_servo_angle(CH_2, safe_sqrt(2), 45.0, 1.0);
149 0 : add_servo_angle(CH_3, 1.0, 180.0, 1.0);
150 0 : break;
151 : }
152 :
153 859845 : case SWASHPLATE_TYPE_H3_120:
154 : {
155 : // three-servo roll/pitch mixer for H3-120
156 : // HR3-120 uses reversed servo and collective direction in heli setup
157 : // not a pure mixing swashplate, phase angle is adjustable
158 859845 : add_servo_angle(CH_1, 1.0, -60.0, 1.0);
159 859845 : add_servo_angle(CH_2, 1.0, 60.0, 1.0);
160 859845 : add_servo_angle(CH_3, 1.0, 180.0, 1.0);
161 859845 : break;
162 : }
163 :
164 0 : case SWASHPLATE_TYPE_H4_90:
165 : {
166 : // four-servo roll/pitch mixer for H4-90
167 : // 1:1 pure input style, phase angle is adjustable
168 : // servos 3 & 7 are elevator
169 : // can also be used for all versions of 90 deg three-servo swashplates
170 0 : add_servo_angle(CH_1, 1.0, -90.0, 1.0);
171 0 : add_servo_angle(CH_2, 1.0, 90.0, 1.0);
172 0 : add_servo_angle(CH_3, 1.0, 180.0, 1.0);
173 0 : add_servo_angle(CH_4, 1.0, 0.0, 1.0);
174 0 : break;
175 : }
176 :
177 0 : case SWASHPLATE_TYPE_H4_45:
178 : {
179 : // four-servo roll/pitch mixer for H4-45
180 : // 1:1 pure input style, phase angle is adjustable
181 : // for 45 deg plates servos 1&2 are LF&RF, 3&7 are LR&RR.
182 0 : add_servo_angle(CH_1, 1.0, -45.0, 1.0);
183 0 : add_servo_angle(CH_2, 1.0, 45.0, 1.0);
184 0 : add_servo_angle(CH_3, 1.0, -135.0, 1.0);
185 0 : add_servo_angle(CH_4, 1.0, 135.0, 1.0);
186 0 : break;
187 : }
188 : }
189 :
190 859845 : }
191 :
192 2579535 : void AP_MotorsHeli_Swash::add_servo_angle(uint8_t num, float scale, float angle, float collective)
193 : {
194 2579535 : float phase_angle = constrain_float(_phase_angle, -30, 30);
195 5159070 : add_servo_raw(num,
196 2579535 : scale * cosf(radians(angle + 90 - phase_angle)),
197 2579535 : scale * cosf(radians(angle - phase_angle)),
198 : collective);
199 2579535 : }
200 :
201 2579535 : void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, float collective)
202 : {
203 2579535 : if (num >= _max_num_servos) {
204 : // Indexing problem should never happen
205 0 : return;
206 : }
207 :
208 2579535 : _enabled[num] = true;
209 2579535 : _rollFactor[num] = roll * 0.45;
210 2579535 : _pitchFactor[num] = pitch * 0.45;
211 2579535 : _collectiveFactor[num] = collective;
212 :
213 : // Setup output function
214 2579535 : SRV_Channel::Function function = SRV_Channels::get_motor_function(_motor_num[num]);
215 2579535 : SRV_Channels::set_aux_channel_default(function, _motor_num[num]);
216 :
217 : // outputs are defined on a -500 to 500 range for swash servos
218 2579535 : SRV_Channels::set_range(function, 1000);
219 :
220 : // swash servos always use full endpoints as restricting them would lead to scaling errors
221 2579535 : SRV_Channels::set_output_min_max(function, 1000, 2000);
222 :
223 : }
224 :
225 : // calculates servo output
226 3052397 : void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective)
227 : {
228 :
229 : // Store inputs for logging, store col before col reversal to ensure logging comes out with the correct sign (+/-)
230 3052397 : _roll_input = roll;
231 3052397 : _pitch_input = pitch;
232 3052397 : _collective_input_scaled = collective;
233 :
234 : // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch
235 3052397 : if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED) {
236 0 : collective = 1 - collective;
237 : }
238 :
239 15261985 : for (uint8_t i = 0; i < _max_num_servos; i++) {
240 12209588 : if (!_enabled[i]) {
241 : // This servo is not enabled
242 3052397 : continue;
243 : }
244 :
245 9157191 : _output[i] = (_rollFactor[i] * roll) + (_pitchFactor[i] * pitch) + _collectiveFactor[i] * collective;
246 9157191 : if (_swash_type == SWASHPLATE_TYPE_H1 && (i == CH_1 || i == CH_2)) {
247 0 : _output[i] += 0.5f;
248 : }
249 :
250 : // rescale from -1..1, so we can use the pwm calc that includes trim
251 9157191 : _output[i] = 2.0f * _output[i] - 1.0f;
252 :
253 9157191 : if (_make_servo_linear) {
254 0 : _output[i] = get_linear_servo_output(_output[i]);
255 : }
256 :
257 : }
258 3052397 : }
259 :
260 : // set_linear_servo_out - sets swashplate servo output to be linear
261 0 : float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
262 : {
263 :
264 0 : input = constrain_float(input, -1.0f, 1.0f);
265 :
266 : //servo output is calculated by normalizing input to 50 deg arm rotation as full input for a linear throw
267 0 : return safe_asin(0.766044f * input) * 1.145916;
268 :
269 : }
270 :
271 : // Output calculated values to servos
272 3052397 : void AP_MotorsHeli_Swash::output()
273 : {
274 15261985 : for (uint8_t i = 0; i < _max_num_servos; i++) {
275 12209588 : if (_enabled[i]) {
276 9157191 : rc_write(_motor_num[i], _output[i]);
277 : }
278 : }
279 3052397 : }
280 :
281 : // convert input in -1 to +1 range to pwm output for swashplate servo.
282 : // The value 0 corresponds to the trim value of the servo. Swashplate
283 : // servo travel range is fixed to 1000 pwm and therefore the input is
284 : // multiplied by 500 to get PWM output.
285 9157191 : void AP_MotorsHeli_Swash::rc_write(uint8_t chan, float swash_in)
286 : {
287 9157191 : uint16_t pwm = (uint16_t)(1500 + 500 * swash_in);
288 9157191 : SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
289 9157191 : SRV_Channels::set_output_pwm_trimmed(function, pwm);
290 9157191 : }
291 :
292 : // Get function output mask
293 160 : uint32_t AP_MotorsHeli_Swash::get_output_mask() const
294 : {
295 160 : uint32_t mask = 0;
296 800 : for (uint8_t i = 0; i < _max_num_servos; i++) {
297 640 : if (_enabled[i]) {
298 240 : mask |= 1U < _motor_num[i];
299 : }
300 : }
301 160 : return mask;
302 : }
303 :
304 : #if HAL_LOGGING_ENABLED
305 : // Write SWSH log for this instance of swashplate
306 76247 : void AP_MotorsHeli_Swash::write_log(float cyclic_scaler, float col_ang_min, float col_ang_max, int16_t col_min, int16_t col_max) const
307 : {
308 : // Calculate the collective contribution to blade pitch angle
309 : // Swashplate receives the scaled collective value based on the col_min and col_max params. We have to reverse the scaling here to do the angle calculation.
310 76247 : float collective_scalar = ((float)(col_max-col_min))*1e-3;
311 76247 : collective_scalar = MAX(collective_scalar, 1e-3);
312 76247 : float _collective_input = (_collective_input_scaled - (float)(col_min - 1000)*1e-3) / collective_scalar;
313 76247 : float col = (col_ang_max - col_ang_min) * _collective_input + col_ang_min;
314 :
315 : // Calculate the cyclic contribution to blade pitch angle
316 76247 : float tcyc = norm(_roll_input, _pitch_input) * cyclic_scaler;
317 76247 : float pcyc = _pitch_input * cyclic_scaler;
318 76247 : float rcyc = _roll_input * cyclic_scaler;
319 :
320 : // @LoggerMessage: SWSH
321 : // @Description: Helicopter swashplate logging
322 : // @Field: TimeUS: Time since system startup
323 : // @Field: I: Swashplate instance
324 : // @Field: Col: Blade pitch angle contribution from collective
325 : // @Field: TCyc: Total blade pitch angle contribution from cyclic
326 : // @Field: PCyc: Blade pitch angle contribution from pitch cyclic
327 : // @Field: RCyc: Blade pitch angle contribution from roll cyclic
328 152494 : AP::logger().WriteStreaming("SWSH", "TimeUS,I,Col,TCyc,PCyc,RCyc", "s#dddd", "F-0000", "QBffff",
329 : AP_HAL::micros64(),
330 76247 : _instance,
331 : col,
332 : tcyc,
333 : pcyc,
334 : rcyc);
335 76247 : }
336 : #endif
|
