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> 52 #include <sys/mount.h> 53 #include <sys/ioctl.h> 56 #include <arch/board/board.h> 62 static void usage(
const char *reason);
69 PX4_ERR(
"%s", reason);
72 PRINT_MODULE_DESCRIPTION(
"Tool for ESC calibration\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" 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)",
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);
100 unsigned max_channels = 0;
102 uint32_t set_mask = 0;
103 unsigned long channels;
104 unsigned single_ch = 0;
114 usage(
"no channels provided");
118 int arg_consumed = 0;
121 const char *myoptarg = NULL;
123 while ((ch = px4_getopt(argc, argv,
"d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) {
133 channels = strtoul(myoptarg, &ep, 0);
135 while ((single_ch = channels % 10)) {
137 set_mask |= 1 << (single_ch - 1);
145 set_mask = strtoul(myoptarg, &ep, 0);
148 usage(
"bad set_mask value");
165 pwm_low = strtoul(myoptarg, &ep, 0);
172 usage(
"low PWM invalid");
180 pwm_high = strtoul(myoptarg, &ep, 0);
183 usage(
"high PWM invalid");
196 usage(
"no channels chosen");
200 if (pwm_low > pwm_high) {
201 usage(
"low pwm is higher than high pwm");
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");
226 printf(
"\nATTENTION, please remove or fix propellers before starting calibration!\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" 233 "Do you want to start calibration now: y or n?\n");
238 ret = poll(&fds, 1, 0);
241 if (
read(0, &c, 1) <= 0) {
242 printf(
"ESC calibration read error\n");
246 if (c ==
'y' || c ==
'Y') {
249 }
else if (c == 0x03 || c == 0x63 || c ==
'q') {
250 printf(
"ESC calibration exited\n");
253 }
else if (c ==
'n' || c ==
'N') {
254 printf(
"ESC calibration aborted\n");
258 printf(
"Unknown input, ESC calibration aborted\n");
268 int fd = open(dev, 0);
271 PX4_ERR(
"can't open %s", dev);
279 PX4_ERR(
"PWM_SERVO_GET_COUNT");
287 PX4_ERR(
"PWM_SERVO_SET_ARM_OK");
295 PX4_ERR(
"PWM_SERVO_ARM");
299 printf(
"Outputs armed");
303 printf(
"\nHigh PWM set: %d\n" 305 "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" 311 for (
unsigned i = 0; i < max_channels; i++) {
313 if (set_mask & 1 << i) {
317 PX4_ERR(
"PWM_SERVO_SET(%d), value: %d", i, pwm_high);
323 ret = poll(&fds, 1, 0);
326 if (
read(0, &c, 1) <= 0) {
327 printf(
"ESC calibration read error\n");
334 }
else if (c == 0x03 || c == 0x63 || c ==
'q') {
335 printf(
"ESC calibration exited");
344 printf(
"Low PWM set: %d\n" 346 "Hit ENTER when finished\n" 352 for (
unsigned i = 0; i < max_channels; i++) {
353 if (set_mask & 1 << i) {
357 PX4_ERR(
"PWM_SERVO_SET(%d), value: %d", i, pwm_low);
363 ret = poll(&fds, 1, 0);
366 if (
read(0, &c, 1) <= 0) {
367 printf(
"ESC calibration read error\n");
374 }
else if (c == 0x03 || c == 0x63 || c ==
'q') {
375 printf(
"ESC calibration exited\n");
388 PX4_ERR(
"PWM_SERVO_DISARM");
392 printf(
"Outputs disarmed");
394 printf(
"ESC calibration finished\n");
#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)
#define PWM_SERVO_SET_ARM_OK
set the 'ARM ok' 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[])
#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)
#define PWM_OUTPUT0_DEVICE_PATH
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS
#define PWM_OUTPUT_MAX_CHANNELS
static void usage(const char *reason)
int orb_check(int handle, bool *updated)
#define PWM_HIGHEST_MIN
Highest PWM allowed as the minimum PWM.
#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)