PX4 Firmware
PX4 Autopilot Software http://px4.io
drv_pwm_output.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2015, 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file PWM servo output interface.
36  *
37  * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
38  * pwm_output_values structure to the device
39  * Writing a value of 0 to a channel suppresses any output for that
40  * channel.
41  */
42 
43 #pragma once
44 
45 #include <px4_platform_common/defines.h>
46 
47 #include <stdint.h>
48 #include <sys/ioctl.h>
49 #include <board_config.h>
50 
51 #include "drv_orb_dev.h"
52 
54 
55 /**
56  * Path for the default PWM output device.
57  *
58  * Note that on systems with more than one PWM output path (e.g.
59  * PX4FMU with PX4IO connected) there may be other devices that
60  * respond to this protocol.
61  */
62 #define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output"
63 #define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0"
64 
65 #define PWM_OUTPUT_MAX_CHANNELS 16
66 
68  uint32_t channel_count;
70 };
71 
72 /* Use defaults unless the board override the defaults by providing
73  * PX4_PWM_ALTERNATE_RANGES and a replacement set of
74  * constants
75  */
76 #if !defined(PX4_PWM_ALTERNATE_RANGES)
77 
78 /**
79  * Lowest minimum PWM in us
80  */
81 #define PWM_LOWEST_MIN 90
82 
83 /**
84  * Default value for a shutdown motor
85  */
86 #define PWM_MOTOR_OFF 900
87 
88 /**
89  * Default minimum PWM in us
90  */
91 #define PWM_DEFAULT_MIN 1000
92 
93 /**
94  * Highest PWM allowed as the minimum PWM
95  */
96 #define PWM_HIGHEST_MIN 1600
97 
98 /**
99  * Highest maximum PWM in us
100  */
101 #define PWM_HIGHEST_MAX 2150
102 
103 /**
104  * Default maximum PWM in us
105  */
106 #define PWM_DEFAULT_MAX 2000
107 
108 /**
109  * Default trim PWM in us
110  */
111 #define PWM_DEFAULT_TRIM 0
112 
113 /**
114  * Lowest PWM allowed as the maximum PWM
115  */
116 #define PWM_LOWEST_MAX 200
117 
118 #endif // not PX4_PWM_ALTERNATE_RANGES
119 
120 /**
121  * Do not output a channel with this value
122  */
123 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
124 
125 /**
126  * Servo output signal type, value is actual servo output pulse
127  * width in microseconds.
128  */
129 typedef uint16_t servo_position_t;
130 
131 /**
132  * RC config values for a channel
133  *
134  * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
135  * param_get() dependency
136  */
138  uint8_t channel;
139  uint16_t rc_min;
140  uint16_t rc_trim;
141  uint16_t rc_max;
142  uint16_t rc_dz;
143  uint16_t rc_assignment;
145 };
146 
147 /*
148  * ioctl() definitions
149  *
150  * Note that ioctls and ORB updates should not be mixed, as the
151  * behaviour of the system in this case is not defined.
152  */
153 #define _PWM_SERVO_BASE 0x2a00
154 
155 /** arm all servo outputs handle by this driver */
156 #define PWM_SERVO_ARM _PX4_IOC(_PWM_SERVO_BASE, 0)
157 
158 /** disarm all servo outputs (stop generating pulses) */
159 #define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1)
160 
161 /** get default servo update rate */
162 #define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2)
163 
164 /** set alternate servo update rate */
165 #define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3)
166 
167 /** get alternate servo update rate */
168 #define PWM_SERVO_GET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 4)
169 
170 /** get the number of servos in *(unsigned *)arg */
171 #define PWM_SERVO_GET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 5)
172 
173 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
174 #define PWM_SERVO_SET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 6)
175 
176 /** check the selected update rates */
177 #define PWM_SERVO_GET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 7)
178 
179 /** set the 'ARM ok' bit, which activates the safety switch */
180 #define PWM_SERVO_SET_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 8)
181 
182 /** clear the 'ARM ok' bit, which deactivates the safety switch */
183 #define PWM_SERVO_CLEAR_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 9)
184 
185 /** start DSM bind */
186 #define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10)
187 
188 /** power up DSM receiver */
189 #define DSM_BIND_POWER_UP _PX4_IOC(_PWM_SERVO_BASE, 11)
190 
191 /** set the PWM value for failsafe */
192 #define PWM_SERVO_SET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 12)
193 
194 /** get the PWM value for failsafe */
195 #define PWM_SERVO_GET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 13)
196 
197 /** set the PWM value when disarmed - should be no PWM (zero) by default */
198 #define PWM_SERVO_SET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 14)
199 
200 /** get the PWM value when disarmed */
201 #define PWM_SERVO_GET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 15)
202 
203 /** set the minimum PWM value the output will send */
204 #define PWM_SERVO_SET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 16)
205 
206 /** get the minimum PWM value the output will send */
207 #define PWM_SERVO_GET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 17)
208 
209 /** set the maximum PWM value the output will send */
210 #define PWM_SERVO_SET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 18)
211 
212 /** get the maximum PWM value the output will send */
213 #define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19)
214 
215 /** set the TRIM value the output will send */
216 #define PWM_SERVO_SET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 20)
217 
218 /** get the TRIM value the output will send */
219 #define PWM_SERVO_GET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 21)
220 
221 /** set the number of servos in (unsigned)arg - allows change of
222  * split between servos and GPIO */
223 #define PWM_SERVO_SET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 22)
224 
225 /** set the lockdown override flag to enable outputs in HIL */
226 #define PWM_SERVO_SET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 23)
227 
228 /** get the lockdown override flag to enable outputs in HIL */
229 #define PWM_SERVO_GET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 24)
230 
231 /** force safety switch off (to disable use of safety switch) */
232 #define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 25)
233 
234 /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
235 #define PWM_SERVO_SET_FORCE_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 26)
236 
237 /** make failsafe non-recoverable (termination) if it occurs */
238 #define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 27)
239 
240 /** force safety switch on (to enable use of safety switch) */
241 #define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28)
242 
243 /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
244 #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32)
245 
246 /** set SBUS output frame rate in Hz */
247 #define PWM_SERVO_SET_SBUS_RATE _PX4_IOC(_PWM_SERVO_BASE, 33)
248 
249 /** set auxillary output mode. These correspond to enum Mode in px4fmu/fmu.cpp */
250 #define PWM_SERVO_MODE_NONE 0
251 #define PWM_SERVO_MODE_1PWM 1
252 #define PWM_SERVO_MODE_2PWM 2
253 #define PWM_SERVO_MODE_2PWM2CAP 3
254 #define PWM_SERVO_MODE_3PWM 4
255 #define PWM_SERVO_MODE_3PWM1CAP 5
256 #define PWM_SERVO_MODE_4PWM 6
257 #define PWM_SERVO_MODE_4PWM1CAP 7
258 #define PWM_SERVO_MODE_4PWM2CAP 8
259 #define PWM_SERVO_MODE_5PWM 9
260 #define PWM_SERVO_MODE_5PWM1CAP 10
261 #define PWM_SERVO_MODE_6PWM 11
262 #define PWM_SERVO_MODE_8PWM 12
263 #define PWM_SERVO_MODE_14PWM 13
264 #define PWM_SERVO_MODE_4CAP 14
265 #define PWM_SERVO_MODE_5CAP 15
266 #define PWM_SERVO_MODE_6CAP 16
267 #define PWM_SERVO_ENTER_TEST_MODE 17
268 #define PWM_SERVO_EXIT_TEST_MODE 18
269 #define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34)
270 
271 /*
272  *
273  *
274  * WARNING WARNING WARNING! DO NOT EXCEED 47 IN IOC INDICES HERE!
275  *
276  *
277  */
278 
279 /** set a single servo to a specific value */
280 #define PWM_SERVO_SET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x30 + _servo)
281 
282 /** get a single specific servo value */
283 #define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x50 + _servo)
284 
285 /** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
286  * whose update rates must be the same.
287  */
288 #define PWM_SERVO_GET_RATEGROUP(_n) _PX4_IOC(_PWM_SERVO_BASE, 0x70 + _n)
289 
290 /** specific rates for configuring the timer for OneShot or PWM */
291 #define PWM_RATE_ONESHOT 0u
292 #define PWM_RATE_LOWER_LIMIT 1u
293 #define PWM_RATE_UPPER_LIMIT 10000u
294 
295 /** Dshot PWM frequency */
296 #define DSHOT1200 1200000u //Hz
297 #define DSHOT600 600000u //Hz
298 #define DSHOT300 300000u //Hz
299 #define DSHOT150 150000u //Hz
300 
301 #define DSHOT_MAX_THROTTLE 1999
302 
303 typedef enum {
310  DShot_cmd_esc_info, // V2 includes settings
315  DShot_cmd_settings_request, // Currently not implemented
319  DShot_cmd_led0_on, // BLHeli32 only
320  DShot_cmd_led1_on, // BLHeli32 only
321  DShot_cmd_led2_on, // BLHeli32 only
322  DShot_cmd_led3_on, // BLHeli32 only
323  DShot_cmd_led0_off, // BLHeli32 only
324  DShot_cmd_led1_off, // BLHeli32 only
325  DShot_cmd_led2_off, // BLHeli32 only
326  DShot_cmd_led4_off, // BLHeli32 only
327  DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
328  DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
333  // >47 are throttle values
335 
336 
337 /*
338  * Low-level PWM output interface.
339  *
340  * This is the low-level API to the platform-specific PWM driver.
341  */
342 
343 /**
344  * Intialise the PWM servo outputs using the specified configuration.
345  *
346  * @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
347  * This allows some of the channels to remain configured
348  * as GPIOs or as another function.
349  * @return OK on success.
350  */
351 __EXPORT extern int up_pwm_servo_init(uint32_t channel_mask);
352 
353 /**
354  * De-initialise the PWM servo outputs.
355  */
356 __EXPORT extern void up_pwm_servo_deinit(void);
357 
358 /**
359  * Arm or disarm servo outputs.
360  *
361  * When disarmed, servos output no pulse.
362  *
363  * @bug This function should, but does not, guarantee that any pulse
364  * currently in progress is cleanly completed.
365  *
366  * @param armed If true, outputs are armed; if false they
367  * are disarmed.
368  */
369 __EXPORT extern void up_pwm_servo_arm(bool armed);
370 
371 /**
372  * Set the servo update rate for all rate groups.
373  *
374  * @param rate The update rate in Hz to set.
375  * @return OK on success, -ERANGE if an unsupported update rate is set.
376  */
377 __EXPORT extern int up_pwm_servo_set_rate(unsigned rate);
378 
379 /**
380  * Get a bitmap of output channels assigned to a given rate group.
381  *
382  * @param group The rate group to query. Rate groups are assigned contiguously
383  * starting from zero.
384  * @return A bitmap of channels assigned to the rate group, or zero if
385  * the group number has no channels.
386  */
387 __EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
388 
389 /**
390  * Set the update rate for a given rate group.
391  *
392  * @param group The rate group whose update rate will be changed.
393  * @param rate The update rate in Hz.
394  * @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
395  */
396 __EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
397 
398 /**
399  * Trigger all timer's channels in Oneshot mode to fire
400  * the oneshot with updated values.
401  * Nothing is done if not in oneshot mode.
402  *
403  */
404 __EXPORT extern void up_pwm_update(void);
405 
406 /**
407  * Set the current output value for a channel.
408  *
409  * @param channel The channel to set.
410  * @param value The output pulse width in microseconds.
411  */
412 __EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value);
413 
414 /**
415  * Get the current output value for a channel.
416  *
417  * @param channel The channel to read.
418  * @return The output pulse width in microseconds, or zero if
419  * outputs are not armed or not configured.
420  */
421 __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
422 
423 /**
424  * Intialise the Dshot outputs using the specified configuration.
425  *
426  * @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
427  * This allows some of the channels to remain configured
428  * as GPIOs or as another function.
429  * @param dshot_pwm_freq is frequency of DSHOT signal. Usually DSHOT1200, DSHOT600, DSHOT300 or DSHOT150
430  * @return OK on success.
431  */
432 __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
433 
434 /**
435  * Set the current dshot throttle value for a channel (motor).
436  *
437  * @param channel The channel to set.
438  * @param throttle The output dshot throttle value in [0, 1999 = DSHOT_MAX_THROTTLE].
439  * @param telemetry If true, request telemetry from that motor
440  */
441 __EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
442 
443 /**
444  * Send DShot command to a channel (motor).
445  *
446  * @param channel The channel to set.
447  * @param command dshot_command_t
448  * @param telemetry If true, request telemetry from that motor
449  */
450 __EXPORT extern void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry);
451 
452 /**
453  * Trigger dshot data transfer.
454  */
455 __EXPORT extern void up_dshot_trigger(void);
456 
457 /**
458  * Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.).
459  *
460  * When disarmed, dshot output no pulse.
461  *
462  * @param armed If true, outputs are armed; if false they
463  * are disarmed.
464  */
465 __EXPORT extern int up_dshot_arm(bool armed);
466 
__EXPORT void up_dshot_trigger(void)
Trigger dshot data transfer.
__EXPORT servo_position_t up_pwm_servo_get(unsigned channel)
Get the current output value for a channel.
uint16_t servo_position_t
Servo output signal type, value is actual servo output pulse width in microseconds.
#define __END_DECLS
Definition: visibility.h:59
__EXPORT uint32_t up_pwm_servo_get_rate_group(unsigned group)
Get a bitmap of output channels assigned to a given rate group.
uORB published object driver.
RC config values for a channel.
__EXPORT void up_pwm_servo_deinit(void)
De-initialise the PWM servo outputs.
Definition: I2C.hpp:51
__EXPORT int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
Intialise the Dshot outputs using the specified configuration.
__EXPORT int up_dshot_arm(bool armed)
Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.).
__EXPORT int up_pwm_servo_set_rate(unsigned rate)
Set the servo update rate for all rate groups.
uint16_t values[PWM_OUTPUT_MAX_CHANNELS]
#define __BEGIN_DECLS
Definition: visibility.h:58
__EXPORT int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
Set the update rate for a given rate group.
dshot_command_t
static struct actuator_armed_s armed
Definition: Commander.cpp:139
uint32_t channel_count
#define PWM_OUTPUT_MAX_CHANNELS
__EXPORT int up_pwm_servo_set(unsigned channel, servo_position_t value)
Set the current output value for a channel.
__EXPORT void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
Send DShot command to a channel (motor).
__EXPORT void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
Set the current dshot throttle value for a channel (motor).
__EXPORT int up_pwm_servo_init(uint32_t channel_mask)
Intialise the PWM servo outputs using the specified configuration.
__EXPORT void up_pwm_servo_arm(bool armed)
Arm or disarm servo outputs.
__EXPORT void up_pwm_update(void)
Trigger all timer&#39;s channels in Oneshot mode to fire the oneshot with updated values.