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 <AP_HAL/AP_HAL.h>
17 : #include <AP_Math/AP_Math.h>
18 : #include "AP_MotorsSingle.h"
19 : #include <GCS_MAVLink/GCS.h>
20 : #include <SRV_Channel/SRV_Channel.h>
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 : // init
25 1 : void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
26 : {
27 : // make sure 6 output channels are mapped
28 7 : for (uint8_t i = 0; i < 6; i++) {
29 6 : add_motor_num(CH_1 + i);
30 : }
31 :
32 : // set the motor_enabled flag so that the main ESC can be calibrated like other frame types
33 1 : motor_enabled[AP_MOTORS_MOT_5] = true;
34 1 : motor_enabled[AP_MOTORS_MOT_6] = true;
35 :
36 : // setup actuator scaling
37 5 : for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
38 4 : SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
39 : }
40 :
41 1 : _mav_type = MAV_TYPE_COAXIAL;
42 :
43 : // record successful initialisation if what we setup was the desired frame_class
44 1 : set_initialised_ok(frame_class == MOTOR_FRAME_SINGLE);
45 1 : }
46 :
47 : // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
48 41 : void AP_MotorsSingle::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
49 : {
50 : // nothing to do
51 41 : }
52 :
53 : // set update rate to motors - a value in hertz
54 1 : void AP_MotorsSingle::set_update_rate(uint16_t speed_hz)
55 : {
56 : // record requested speed
57 1 : _speed_hz = speed_hz;
58 :
59 1 : uint32_t mask =
60 : 1U << AP_MOTORS_MOT_5 |
61 : 1U << AP_MOTORS_MOT_6 ;
62 1 : rc_set_freq(mask, _speed_hz);
63 1 : }
64 :
65 50349 : void AP_MotorsSingle::output_to_motors()
66 : {
67 50349 : if (!initialised_ok()) {
68 0 : return;
69 : }
70 50349 : switch (_spool_state) {
71 17703 : case SpoolState::SHUT_DOWN:
72 : // sends minimum values out to the motors
73 17703 : rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
74 17703 : rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
75 17703 : rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
76 17703 : rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
77 17703 : rc_write(AP_MOTORS_MOT_5, output_to_pwm(0));
78 17703 : rc_write(AP_MOTORS_MOT_6, output_to_pwm(0));
79 17703 : break;
80 205 : case SpoolState::GROUND_IDLE:
81 : // sends output to motors when armed but not flying
82 1025 : for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
83 820 : rc_write_angle(AP_MOTORS_MOT_1 + i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
84 : }
85 205 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], actuator_spin_up_to_ground_idle());
86 205 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], actuator_spin_up_to_ground_idle());
87 205 : rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[AP_MOTORS_MOT_5]));
88 205 : rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[AP_MOTORS_MOT_6]));
89 205 : break;
90 32441 : case SpoolState::SPOOLING_UP:
91 : case SpoolState::THROTTLE_UNLIMITED:
92 : case SpoolState::SPOOLING_DOWN:
93 : // set motor output based on thrust requests
94 162205 : for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
95 129764 : rc_write_angle(AP_MOTORS_MOT_1 + i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
96 : }
97 32441 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_5], thr_lin.thrust_to_actuator(_thrust_out));
98 32441 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_6], thr_lin.thrust_to_actuator(_thrust_out));
99 32441 : rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[AP_MOTORS_MOT_5]));
100 32441 : rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[AP_MOTORS_MOT_6]));
101 32441 : break;
102 : }
103 : }
104 :
105 : // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
106 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
107 44 : uint32_t AP_MotorsSingle::get_motor_mask()
108 : {
109 44 : uint32_t motor_mask =
110 : 1U << AP_MOTORS_MOT_5 |
111 : 1U << AP_MOTORS_MOT_6;
112 :
113 44 : uint32_t mask = motor_mask_to_srv_channel_mask(motor_mask);
114 :
115 : // add parent's mask
116 44 : mask |= AP_MotorsMulticopter::get_motor_mask();
117 :
118 44 : return mask;
119 : }
120 :
121 : // sends commands to the motors
122 50349 : void AP_MotorsSingle::output_armed_stabilizing()
123 : {
124 : float roll_thrust; // roll thrust input value, +/- 1.0
125 : float pitch_thrust; // pitch thrust input value, +/- 1.0
126 : float yaw_thrust; // yaw thrust input value, +/- 1.0
127 : float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
128 : float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
129 : float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
130 : float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
131 50349 : float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
132 50349 : float actuator_allowed = 0.0f; // amount of yaw we can fit in
133 : float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
134 50349 : float actuator_max = 0.0f; // maximum actuator value
135 :
136 : // apply voltage and air pressure compensation
137 50349 : const float compensation_gain = thr_lin.get_compensation_gain();
138 50349 : roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
139 50349 : pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
140 50349 : yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
141 50349 : throttle_thrust = get_throttle() * compensation_gain;
142 50349 : throttle_avg_max = _throttle_avg_max * compensation_gain;
143 :
144 : // sanity check throttle is above zero and below current limited throttle
145 50349 : if (throttle_thrust <= 0.0f) {
146 18511 : throttle_thrust = 0.0f;
147 18511 : limit.throttle_lower = true;
148 : }
149 50349 : if (throttle_thrust >= _throttle_thrust_max) {
150 18020 : throttle_thrust = _throttle_thrust_max;
151 18020 : limit.throttle_upper = true;
152 : }
153 :
154 50349 : throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
155 :
156 50349 : float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
157 :
158 : // calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
159 50349 : if (is_zero(rp_thrust_max)) {
160 1 : rp_scale = 1.0f;
161 : } else {
162 50348 : rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float) _yaw_headroom * 0.001f)) / rp_thrust_max, 0.0f, 1.0f);
163 50348 : if (rp_scale < 1.0f) {
164 0 : limit.roll = true;
165 0 : limit.pitch = true;
166 : }
167 : }
168 :
169 50349 : actuator_allowed = 1.0f - rp_scale * rp_thrust_max;
170 50349 : if (fabsf(yaw_thrust) > actuator_allowed) {
171 2213 : yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
172 2213 : limit.yaw = true;
173 : }
174 :
175 : // combine roll, pitch and yaw on each actuator
176 : // front servo
177 50349 : actuator[0] = rp_scale * roll_thrust - yaw_thrust;
178 : // right servo
179 50349 : actuator[1] = rp_scale * pitch_thrust - yaw_thrust;
180 : // rear servo
181 50349 : actuator[2] = -rp_scale * roll_thrust - yaw_thrust;
182 : // left servo
183 50349 : actuator[3] = -rp_scale * pitch_thrust - yaw_thrust;
184 :
185 : // calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
186 50349 : thrust_min_rpy = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
187 :
188 50349 : thr_adj = throttle_thrust - throttle_avg_max;
189 50349 : if (thr_adj < (thrust_min_rpy - throttle_avg_max)) {
190 : // Throttle can't be reduced to the desired level because this would mean roll or pitch control
191 : // would not be able to reach the desired level because of lack of thrust.
192 43434 : thr_adj = MIN(thrust_min_rpy, throttle_avg_max) - throttle_avg_max;
193 : }
194 :
195 : // calculate the throttle setting for the lift fan
196 50349 : _thrust_out = throttle_avg_max + thr_adj;
197 : // compensation_gain can never be zero
198 50349 : _throttle_out = _thrust_out / compensation_gain;
199 :
200 50349 : if (is_zero(_thrust_out)) {
201 17909 : limit.set_rpy(true);
202 : }
203 :
204 : // limit thrust out for calculation of actuator gains
205 50349 : float thrust_out_actuator = constrain_float(MAX(_throttle_hover * 0.5f, _thrust_out), 0.5f, 1.0f);
206 :
207 : // calculate the maximum allowed actuator output and maximum requested actuator output
208 251745 : for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
209 201396 : if (actuator_max > fabsf(actuator[i])) {
210 0 : actuator_max = fabsf(actuator[i]);
211 : }
212 : }
213 50349 : if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
214 : // roll, pitch and yaw request can not be achieved at full servo defection
215 : // reduce roll, pitch and yaw to reduce the requested defection to maximum
216 0 : limit.set_rpy(true);
217 0 : rp_scale = thrust_out_actuator / actuator_max;
218 : } else {
219 50349 : rp_scale = 1.0f;
220 : }
221 :
222 : // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
223 : // static thrust is proportional to the airflow velocity squared
224 : // therefore the torque of the roll and pitch actuators should be approximately proportional to
225 : // the angle of attack multiplied by the static thrust.
226 251745 : for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
227 201396 : _actuator_out[i] = constrain_float(rp_scale * actuator[i] / thrust_out_actuator, -1.0f, 1.0f);
228 : }
229 50349 : }
230 :
231 : // output_test_seq - spin a motor at the pwm value specified
232 : // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
233 : // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
234 0 : void AP_MotorsSingle::_output_test_seq(uint8_t motor_seq, int16_t pwm)
235 : {
236 : // output to motors and servos
237 0 : switch (motor_seq) {
238 0 : case 1:
239 : // flap servo 1
240 0 : rc_write(AP_MOTORS_MOT_1, pwm);
241 0 : break;
242 0 : case 2:
243 : // flap servo 2
244 0 : rc_write(AP_MOTORS_MOT_2, pwm);
245 0 : break;
246 0 : case 3:
247 : // flap servo 3
248 0 : rc_write(AP_MOTORS_MOT_3, pwm);
249 0 : break;
250 0 : case 4:
251 : // flap servo 4
252 0 : rc_write(AP_MOTORS_MOT_4, pwm);
253 0 : break;
254 0 : case 5:
255 : // spin motor 1
256 0 : rc_write(AP_MOTORS_MOT_5, pwm);
257 0 : break;
258 0 : case 6:
259 : // spin motor 2
260 0 : rc_write(AP_MOTORS_MOT_6, pwm);
261 0 : break;
262 0 : default:
263 : // do nothing
264 0 : break;
265 : }
266 0 : }
|
