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_Motors_Class.h"
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <SRV_Channel/SRV_Channel.h>
19 : #include <GCS_MAVLink/GCS.h>
20 : #include <AP_Notify/AP_Notify.h>
21 : #include <AP_Logger/AP_Logger.h>
22 : #include <cstdint>
23 :
24 : #define AP_MOTORS_SLEW_FILTER_CUTOFF 50.0f
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 : // singleton instance
29 : AP_Motors *AP_Motors::_singleton;
30 :
31 : // Constructor
32 1285 : AP_Motors::AP_Motors(uint16_t speed_hz) :
33 1285 : _speed_hz(speed_hz),
34 1285 : _throttle_filter(),
35 1285 : _throttle_slew(),
36 1285 : _throttle_slew_filter(),
37 1285 : _spool_desired(DesiredSpoolState::SHUT_DOWN),
38 2570 : _spool_state(SpoolState::SHUT_DOWN)
39 : {
40 1285 : _singleton = this;
41 :
42 : // setup throttle filtering
43 1285 : _throttle_filter.set_cutoff_frequency(0.0f);
44 1285 : _throttle_filter.reset(0.0f);
45 :
46 1285 : _throttle_slew_filter.set_cutoff_frequency(AP_MOTORS_SLEW_FILTER_CUTOFF);
47 1285 : _throttle_slew_filter.reset(0.0f);
48 :
49 : // setup throttle slew detector
50 1285 : _throttle_slew.reset();
51 :
52 : // init limit flags
53 1285 : limit.set_all(true);
54 1285 : _thrust_boost = false;
55 1285 : _thrust_balanced = true;
56 1285 : };
57 :
58 3477 : void AP_Motors::get_frame_and_type_string(char *buffer, uint8_t buflen) const
59 : {
60 3477 : const char *frame_str = get_frame_string();
61 3477 : const char *type_str = get_type_string();
62 3477 : if (type_str != nullptr && strlen(type_str)
63 : #if AP_SCRIPTING_ENABLED
64 3201 : && custom_frame_string == nullptr
65 : #endif
66 : ) {
67 3201 : hal.util->snprintf(buffer, buflen, "Frame: %s/%s", frame_str, type_str);
68 : } else {
69 276 : hal.util->snprintf(buffer, buflen, "Frame: %s", frame_str);
70 : }
71 3477 : }
72 :
73 1668 : void AP_Motors::armed(bool arm)
74 : {
75 1668 : if (_armed != arm) {
76 1668 : _armed = arm;
77 1668 : AP_Notify::flags.armed = arm;
78 1668 : if (!arm) {
79 828 : save_params_on_disarm();
80 : }
81 : }
82 1668 : };
83 :
84 : // pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
85 3748409 : void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input)
86 : {
87 3748409 : _roll_radio_passthrough = roll_input;
88 3748409 : _pitch_radio_passthrough = pitch_input;
89 3748409 : _throttle_radio_passthrough = throttle_input;
90 3748409 : _yaw_radio_passthrough = yaw_input;
91 3748409 : }
92 :
93 : /*
94 : write to an output channel
95 : */
96 155585226 : void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
97 : {
98 155585226 : SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
99 155585226 : if ((1U<<chan) & _motor_pwm_scaled.mask) {
100 : // note that PWM_MIN/MAX has been forced to 1000/2000
101 0 : SRV_Channels::set_output_scaled(function, float(pwm) - _motor_pwm_scaled.offset);
102 : } else {
103 155585226 : SRV_Channels::set_output_pwm(function, pwm);
104 : }
105 155585226 : }
106 :
107 : /*
108 : write to an output channel for an angle actuator
109 : */
110 4559042 : void AP_Motors::rc_write_angle(uint8_t chan, int16_t angle_cd)
111 : {
112 4559042 : SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
113 4559042 : SRV_Channels::set_output_scaled(function, angle_cd);
114 4559042 : }
115 :
116 : /*
117 : set frequency of a set of channels
118 : */
119 2565 : void AP_Motors::rc_set_freq(uint32_t motor_mask, uint16_t freq_hz)
120 : {
121 2565 : if (freq_hz > 50) {
122 2565 : _motor_fast_mask |= motor_mask;
123 : }
124 :
125 2565 : const uint32_t mask = motor_mask_to_srv_channel_mask(motor_mask);
126 2565 : hal.rcout->set_freq(mask, freq_hz);
127 2565 : hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
128 :
129 2565 : const PWMType type = _pwm_type;
130 2565 : switch (type) {
131 0 : case PWMType::ONESHOT:
132 0 : if (freq_hz > 50 && mask != 0) {
133 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
134 : }
135 0 : break;
136 0 : case PWMType::ONESHOT125:
137 0 : if (freq_hz > 50 && mask != 0) {
138 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT125);
139 : }
140 0 : break;
141 0 : case PWMType::BRUSHED:
142 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
143 0 : break;
144 0 : case PWMType::DSHOT150:
145 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT150);
146 0 : break;
147 0 : case PWMType::DSHOT300:
148 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT300);
149 0 : break;
150 0 : case PWMType::DSHOT600:
151 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT600);
152 0 : break;
153 0 : case PWMType::DSHOT1200:
154 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT1200);
155 0 : break;
156 0 : case PWMType::PWM_RANGE:
157 : case PWMType::PWM_ANGLE:
158 : /*
159 : this is a motor output type for multirotors which honours
160 : the SERVOn_MIN/MAX (and TRIM for angle) values per channel
161 :
162 : Motor PWM min and max are hard coded to 1000 to 2000.
163 : Range type offsets by 1000 to get 0 to 1000.
164 : Angle type offsets by 1500 to get -500 to 500.
165 : */
166 :
167 0 : if (type == PWMType::PWM_RANGE) {
168 0 : _motor_pwm_scaled.offset = 1000.0;
169 : } else {
170 : // PWMType::PWM_ANGLE
171 0 : _motor_pwm_scaled.offset = 1500.0;
172 : }
173 :
174 0 : _motor_pwm_scaled.mask |= motor_mask;
175 0 : for (uint8_t i=0; i<16; i++) {
176 0 : if ((1U<<i) & motor_mask) {
177 0 : if (type == PWMType::PWM_RANGE) {
178 0 : SRV_Channels::set_range(SRV_Channels::get_motor_function(i), 1000);
179 : } else {
180 : // PWMType::PWM_ANGLE
181 0 : SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), 500);
182 : }
183 : }
184 : }
185 0 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_NORMAL);
186 0 : break;
187 2565 : default:
188 2565 : hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_NORMAL);
189 2565 : break;
190 : }
191 2565 : }
192 :
193 : /*
194 : map an internal motor mask to real motor mask, accounting for
195 : SERVOn_FUNCTION mappings, and allowing for multiple outputs per
196 : motor number
197 : */
198 35513 : uint32_t AP_Motors::motor_mask_to_srv_channel_mask(uint32_t mask) const
199 : {
200 35513 : uint32_t mask2 = 0;
201 1171929 : for (uint8_t i = 0; i < 32; i++) {
202 1136416 : uint32_t bit = 1UL << i;
203 1136416 : if (mask & bit) {
204 152680 : SRV_Channel::Function function = SRV_Channels::get_motor_function(i);
205 152680 : mask2 |= SRV_Channels::get_output_channel_mask(function);
206 : }
207 : }
208 35513 : return mask2;
209 : }
210 :
211 : /*
212 : add a motor, setting up default output function as needed
213 : */
214 5270 : void AP_Motors::add_motor_num(int8_t motor_num)
215 : {
216 : // ensure valid motor number is provided
217 5270 : if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
218 5270 : SRV_Channel::Function function = SRV_Channels::get_motor_function(motor_num);
219 5270 : SRV_Channels::set_aux_channel_default(function, motor_num);
220 : }
221 5270 : }
222 :
223 : #if AP_SCRIPTING_ENABLED
224 0 : void AP_Motors::set_external_limits(bool roll, bool pitch, bool yaw, bool throttle_lower, bool throttle_upper)
225 : {
226 0 : external_limits.roll = roll;
227 0 : external_limits.pitch = pitch;
228 0 : external_limits.yaw = yaw;
229 0 : external_limits.throttle_lower = throttle_lower;
230 0 : external_limits.throttle_upper = throttle_upper;
231 0 : }
232 : #endif
233 :
234 : // returns true if the configured PWM type is digital and should have fixed endpoints
235 1199 : bool AP_Motors::is_digital_pwm_type() const
236 : {
237 1199 : switch ((PWMType)_pwm_type) {
238 0 : case PWMType::DSHOT150:
239 : case PWMType::DSHOT300:
240 : case PWMType::DSHOT600:
241 : case PWMType::DSHOT1200:
242 0 : return true;
243 1199 : case PWMType::NORMAL:
244 : case PWMType::ONESHOT:
245 : case PWMType::ONESHOT125:
246 : case PWMType::BRUSHED:
247 : case PWMType::PWM_RANGE:
248 : case PWMType::PWM_ANGLE:
249 1199 : break;
250 : }
251 1199 : return false;
252 : }
253 :
254 : // return string corresponding to frame_class
255 3544 : const char* AP_Motors::get_frame_string() const
256 : {
257 : #if AP_SCRIPTING_ENABLED
258 3544 : if (custom_frame_string != nullptr) {
259 5 : return custom_frame_string;
260 : }
261 : #endif
262 3539 : return _get_frame_string();
263 : }
264 :
265 : #if AP_SCRIPTING_ENABLED
266 : // set custom frame string
267 4 : void AP_Motors::set_frame_string(const char * str) {
268 4 : if (custom_frame_string != nullptr) {
269 0 : return;
270 : }
271 4 : const size_t len = strlen(str)+1;
272 4 : custom_frame_string = NEW_NOTHROW char[len];
273 4 : if (custom_frame_string != nullptr) {
274 4 : strncpy(custom_frame_string, str, len);
275 : }
276 : }
277 : #endif
278 :
279 : #if HAL_LOGGING_ENABLED
280 30412448 : void AP_Motors::Log_Write_SPOL() {
281 :
282 30412448 : const bool spool_state_changed{_spool_state != _logged_spool_state};
283 30412448 : const bool des_spool_state_changed{_spool_desired != _logged_spool_desired};
284 :
285 : // Log only changes.
286 30412448 : if (!spool_state_changed && !des_spool_state_changed) {
287 30408808 : return;
288 : }
289 :
290 3640 : _logged_spool_state = _spool_state;
291 3640 : _logged_spool_desired = _spool_desired;
292 :
293 3640 : const struct log_SPOL pkt {
294 : LOG_PACKET_HEADER_INIT(LOG_SPOL_MSG),
295 3640 : time_us : AP_HAL::micros64(),
296 3640 : spool_state : (uint8_t)_spool_state,
297 3640 : des_spool_state : (uint8_t)_spool_desired
298 3640 : };
299 3640 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
300 :
301 : }
302 : #endif
303 :
304 : // output_test_seq - spin a motor at the pwm value specified
305 : // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
306 : // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
307 : // return true if output was successful, false if not possible
308 11185 : bool AP_Motors::output_test_seq(uint8_t motor_seq, int16_t pwm)
309 : {
310 11185 : if (armed() && _interlock) {
311 11185 : _output_test_seq(motor_seq, pwm);
312 11185 : return true;
313 : }
314 0 : output_min();
315 0 : return false;
316 : }
317 :
318 102945 : bool AP_Motors::arming_checks(size_t buflen, char *buffer) const
319 : {
320 102945 : if (!initialised_ok()) {
321 56 : hal.util->snprintf(buffer, buflen, "Check frame class and type");
322 56 : return false;
323 : }
324 :
325 102889 : return true;
326 : }
327 :
328 8 : bool AP_Motors::motor_test_checks(size_t buflen, char *buffer) const
329 : {
330 : // Must pass base class arming checks (the function above)
331 : // Do not run frame specific arming checks as motor test is less strict
332 : // For example not all the outputs have to be assigned
333 8 : if (!AP_Motors::arming_checks(buflen, buffer)) {
334 0 : return false;
335 : }
336 :
337 : // check if safety switch has been pushed
338 8 : if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
339 0 : hal.util->snprintf(buffer, buflen, "Safety switch");
340 0 : return false;
341 : }
342 :
343 : // check E-Stop is not active
344 8 : if (SRV_Channels::get_emergency_stop()) {
345 0 : hal.util->snprintf(buffer, buflen, "Motor Emergency Stopped");
346 0 : return false;
347 : }
348 :
349 8 : return true;
350 : }
351 :
352 : // set all limits
353 38284631 : void AP_Motors::AP_Motors_limit::set_all(bool flag)
354 : {
355 38284631 : set_rpy(flag);
356 38284631 : set_throttle(flag);
357 38284631 : }
358 :
359 : // set limits for roll pitch yaw
360 52007708 : void AP_Motors::AP_Motors_limit::set_rpy(bool flag)
361 : {
362 52007708 : roll = flag;
363 52007708 : pitch = flag;
364 52007708 : yaw = flag;
365 52007708 : }
366 :
367 : // set limits for throttle upper and lower
368 41523461 : void AP_Motors::AP_Motors_limit::set_throttle(bool flag)
369 : {
370 41523461 : throttle_lower = flag;
371 41523461 : throttle_upper = flag;
372 41523461 : }
373 :
374 : namespace AP {
375 436492 : AP_Motors *motors()
376 : {
377 436492 : return AP_Motors::get_singleton();
378 : }
379 : }
|
