PX4 Firmware
PX4 Autopilot Software http://px4.io
esc_calib.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013 PX4 Development Team. All rights reserved.
4  * Author: Julian Oes <joes@student.ethz.ch>
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file esc_calib.c
37  */
38 
39 #include <px4_platform_common/px4_config.h>
40 #include <px4_platform_common/getopt.h>
41 #include <px4_platform_common/module.h>
42 #include <px4_platform_common/defines.h>
43 #include <px4_platform_common/log.h>
44 
45 #include <stdio.h>
46 #include <stdlib.h>
47 #include <string.h>
48 #include <stdbool.h>
49 #include <unistd.h>
50 #include <fcntl.h>
51 #include <poll.h>
52 #include <sys/mount.h>
53 #include <sys/ioctl.h>
54 #include <sys/stat.h>
55 
56 #include <arch/board/board.h>
57 
58 #include "drivers/drv_pwm_output.h"
59 
61 
62 static void usage(const char *reason);
63 __EXPORT int esc_calib_main(int argc, char *argv[]);
64 
65 static void
66 usage(const char *reason)
67 {
68  if (reason != NULL) {
69  PX4_ERR("%s", reason);
70  }
71 
72  PRINT_MODULE_DESCRIPTION("Tool for ESC calibration\n"
73  "\n"
74  "Calibration procedure (running the command will guide you through it):\n"
75  "- Remove props, power off the ESC's\n"
76  "- Stop attitude and rate controllers: mc_rate_control stop, fw_att_control stop\n"
77  "- Make sure safety is off\n"
78  "- Run this command\n"
79  );
80 
81  PRINT_MODULE_USAGE_NAME_SIMPLE("esc_calib", "command");
82  PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/pwm_output0", "<file:dev>", "Select PWM output device", true);
83  PRINT_MODULE_USAGE_PARAM_INT('l', 1000, 0, 3000, "Low PWM value in us", true);
84  PRINT_MODULE_USAGE_PARAM_INT('h', 2000, 0, 3000, "High PWM value in us", true);
85  PRINT_MODULE_USAGE_PARAM_STRING('c', NULL, NULL, "select channels in the form: 1234 (1 digit per channel, 1=first)",
86  true);
87  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 4096, "Select channels via bitmask (eg. 0xF, 3)", true);
88  PRINT_MODULE_USAGE_PARAM_FLAG('a', "Select all channels", true);
89 }
90 
91 int
92 esc_calib_main(int argc, char *argv[])
93 {
94  const char *dev = PWM_OUTPUT0_DEVICE_PATH;
95  char *ep;
96  int ch;
97  int ret;
98  char c;
99 
100  unsigned max_channels = 0;
101 
102  uint32_t set_mask = 0;
103  unsigned long channels;
104  unsigned single_ch = 0;
105 
106  uint16_t pwm_high = PWM_DEFAULT_MAX;
107  uint16_t pwm_low = PWM_DEFAULT_MIN;
108 
109  struct pollfd fds;
110  fds.fd = 0; /* stdin */
111  fds.events = POLLIN;
112 
113  if (argc < 2) {
114  usage("no channels provided");
115  return 1;
116  }
117 
118  int arg_consumed = 0;
119 
120  int myoptind = 1;
121  const char *myoptarg = NULL;
122 
123  while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) {
124  switch (ch) {
125 
126  case 'd':
127  dev = myoptarg;
128  arg_consumed += 2;
129  break;
130 
131  case 'c':
132  /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
133  channels = strtoul(myoptarg, &ep, 0);
134 
135  while ((single_ch = channels % 10)) {
136 
137  set_mask |= 1 << (single_ch - 1);
138  channels /= 10;
139  }
140 
141  break;
142 
143  case 'm':
144  /* Read in mask directly */
145  set_mask = strtoul(myoptarg, &ep, 0);
146 
147  if (*ep != '\0') {
148  usage("bad set_mask value");
149  return 1;
150  }
151 
152  break;
153 
154  case 'a':
155 
156  /* Choose all channels */
157  for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
158  set_mask |= 1 << i;
159  }
160 
161  break;
162 
163  case 'l':
164  /* Read in custom low value */
165  pwm_low = strtoul(myoptarg, &ep, 0);
166 
167  if (*ep != '\0'
168 #if PWM_LOWEST_MIN > 0
169  || pwm_low < PWM_LOWEST_MIN
170 #endif
171  || pwm_low > PWM_HIGHEST_MIN) {
172  usage("low PWM invalid");
173  return 1;
174  }
175 
176  break;
177 
178  case 'h':
179  /* Read in custom high value */
180  pwm_high = strtoul(myoptarg, &ep, 0);
181 
182  if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) {
183  usage("high PWM invalid");
184  return 1;
185  }
186 
187  break;
188 
189  default:
190  usage(NULL);
191  return 1;
192  }
193  }
194 
195  if (set_mask == 0) {
196  usage("no channels chosen");
197  return 1;
198  }
199 
200  if (pwm_low > pwm_high) {
201  usage("low pwm is higher than high pwm");
202  return 1;
203  }
204 
205  /* make sure no other source is publishing control values now */
206  struct actuator_controls_s actuators;
208 
209  /* clear changed flag */
210  orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
211 
212  /* wait 50 ms */
213  px4_usleep(50000);
214 
215  /* now expect nothing changed on that topic */
216  bool orb_updated;
217  orb_check(act_sub, &orb_updated);
218 
219  if (orb_updated) {
220  PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
221  "\tmc_rate_control stop\n"
222  "\tfw_att_control stop\n");
223  return 1;
224  }
225 
226  printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
227  "\n"
228  "Make sure\n"
229  "\t - that the ESCs are not powered\n"
230  "\t - that safety is off (two short blinks)\n"
231  "\t - that the controllers are stopped\n"
232  "\n"
233  "Do you want to start calibration now: y or n?\n");
234 
235  /* wait for user input */
236  while (1) {
237 
238  ret = poll(&fds, 1, 0);
239 
240  if (ret > 0) {
241  if (read(0, &c, 1) <= 0) {
242  printf("ESC calibration read error\n");
243  return 0;
244  }
245 
246  if (c == 'y' || c == 'Y') {
247  break;
248 
249  } else if (c == 0x03 || c == 0x63 || c == 'q') {
250  printf("ESC calibration exited\n");
251  return 0;
252 
253  } else if (c == 'n' || c == 'N') {
254  printf("ESC calibration aborted\n");
255  return 0;
256 
257  } else {
258  printf("Unknown input, ESC calibration aborted\n");
259  return 0;
260  }
261  }
262 
263  /* rate limit to ~ 20 Hz */
264  px4_usleep(50000);
265  }
266 
267  /* open for ioctl only */
268  int fd = open(dev, 0);
269 
270  if (fd < 0) {
271  PX4_ERR("can't open %s", dev);
272  return 1;
273  }
274 
275  /* get number of channels available on the device */
276  ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
277 
278  if (ret != OK) {
279  PX4_ERR("PWM_SERVO_GET_COUNT");
280  goto cleanup;
281  }
282 
283  /* tell IO/FMU that its ok to disable its safety with the switch */
284  ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
285 
286  if (ret != OK) {
287  PX4_ERR("PWM_SERVO_SET_ARM_OK");
288  goto cleanup;
289  }
290 
291  /* tell IO/FMU that the system is armed (it will output values if safety is off) */
292  ret = ioctl(fd, PWM_SERVO_ARM, 0);
293 
294  if (ret != OK) {
295  PX4_ERR("PWM_SERVO_ARM");
296  goto cleanup;
297  }
298 
299  printf("Outputs armed");
300 
301 
302  /* wait for user confirmation */
303  printf("\nHigh PWM set: %d\n"
304  "\n"
305  "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
306  "\n", pwm_high);
307  fflush(stdout);
308 
309  while (1) {
310  /* set max PWM */
311  for (unsigned i = 0; i < max_channels; i++) {
312 
313  if (set_mask & 1 << i) {
314  ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high);
315 
316  if (ret != OK) {
317  PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm_high);
318  goto cleanup;
319  }
320  }
321  }
322 
323  ret = poll(&fds, 1, 0);
324 
325  if (ret > 0) {
326  if (read(0, &c, 1) <= 0) {
327  printf("ESC calibration read error\n");
328  goto done;
329  }
330 
331  if (c == 13) {
332  break;
333 
334  } else if (c == 0x03 || c == 0x63 || c == 'q') {
335  printf("ESC calibration exited");
336  goto done;
337  }
338  }
339 
340  /* rate limit to ~ 20 Hz */
341  px4_usleep(50000);
342  }
343 
344  printf("Low PWM set: %d\n"
345  "\n"
346  "Hit ENTER when finished\n"
347  "\n", pwm_low);
348 
349  while (1) {
350 
351  /* set disarmed PWM */
352  for (unsigned i = 0; i < max_channels; i++) {
353  if (set_mask & 1 << i) {
354  ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
355 
356  if (ret != OK) {
357  PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm_low);
358  goto cleanup;
359  }
360  }
361  }
362 
363  ret = poll(&fds, 1, 0);
364 
365  if (ret > 0) {
366  if (read(0, &c, 1) <= 0) {
367  printf("ESC calibration read error\n");
368  goto done;
369  }
370 
371  if (c == 13) {
372  break;
373 
374  } else if (c == 0x03 || c == 0x63 || c == 'q') {
375  printf("ESC calibration exited\n");
376  goto done;
377  }
378  }
379 
380  /* rate limit to ~ 20 Hz */
381  px4_usleep(50000);
382  }
383 
384  /* disarm */
385  ret = ioctl(fd, PWM_SERVO_DISARM, 0);
386 
387  if (ret != OK) {
388  PX4_ERR("PWM_SERVO_DISARM");
389  goto cleanup;
390  }
391 
392  printf("Outputs disarmed");
393 
394  printf("ESC calibration finished\n");
395 
396 done:
397  close(fd);
398  return 0;
399 cleanup:
400  close(fd);
401  return 1;
402 }
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
#define PWM_LOWEST_MAX
Lowest PWM allowed as the maximum PWM.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
#define PWM_SERVO_SET_ARM_OK
set the &#39;ARM ok&#39; bit, which activates the safety switch
#define PWM_SERVO_GET_COUNT
get the number of servos in *(unsigned *)arg
#define PWM_SERVO_ARM
arm all servo outputs handle by this driver
__EXPORT int esc_calib_main(int argc, char *argv[])
Definition: esc_calib.c:92
Definition: I2C.hpp:51
#define PWM_SERVO_SET(_servo)
set a single servo to a specific value
static void read(bootloader_app_shared_t *pshared)
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define PWM_OUTPUT0_DEVICE_PATH
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS
Definition: uORB.h:256
#define PWM_OUTPUT_MAX_CHANNELS
static void usage(const char *reason)
Definition: esc_calib.c:66
int fd
Definition: dataman.cpp:146
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
#define PWM_HIGHEST_MIN
Highest PWM allowed as the minimum PWM.
#define OK
Definition: uavcan_main.cpp:71
#define PWM_HIGHEST_MAX
Highest maximum PWM in us.
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
#define PWM_LOWEST_MIN
Lowest minimum PWM in us.
#define PWM_SERVO_DISARM
disarm all servo outputs (stop generating pulses)