esc_calib: get disarmed/max values from PWM device, more informative output

src/systemcmds/esc_calib/esc_calib.c

@@ -72,6 +72,7 @@ usage(const char *reason)
 {
 	if (reason != NULL)
 		warnx("%s", reason);
+
 	errx(1,
 	     "usage:\n"
 	     "esc_calib [-d <device>] <channels>\n"
@@ -122,6 +123,7 @@ esc_calib_main(int argc, char *argv[])
 		if (*ep == '\0') {
 			if (channel_number > MAX_CHANNELS || channel_number <= 0) {
 				err(1, "invalid channel number: %d", channel_number);
+
 			} else {
 				channels_selected[channel_number - 1] = true;
 
@@ -142,101 +144,128 @@ esc_calib_main(int argc, char *argv[])
 	while (1) {
 
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 'y' || c == 'Y') {
 
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				printf("ESC calibration exited\n");
 				exit(0);
+
 			} else if (c == 'n' || c == 'N') {
 				printf("ESC calibration aborted\n");
 				exit(0);
+
 			} else {
 				printf("Unknown input, ESC calibration aborted\n");
 				exit(0);
 			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
 	/* open for ioctl only */
 	int fd = open(dev, 0);
+
 	if (fd < 0)
 		err(1, "can't open %s", dev);
 
+	/* get max PWM value setting */
+	uint16_t pwm_max = 0;
+	ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max);
+
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_MAX_PWM");
+
+	/* get disarmed PWM value setting */
+	uint16_t pwm_disarmed = 0;
+	ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed);
 
-	/* Wait for user confirmation */
+	if (ret != OK)
-	printf("\nHigh PWM set\n"
+		err(1, "PWM_SERVO_GET_DISARMED_PWM");
+
+	/* wait for user confirmation */
+	printf("\nHigh PWM set: %d\n"
 	       "\n"
 	       "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
-		"\n");
+	       "\n", pwm_max);
 	fflush(stdout);
 
 	while (1) {
-
+		/* set max PWM */
-		/* First set high PWM */
 		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
 			if (channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max);
+
 				if (ret != OK)
 					err(1, "PWM_SERVO_SET(%d)", i);
 			}
 		}
 
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 13) {
 
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				warnx("ESC calibration exited");
 				exit(0);
 			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
-	/* we don't need any more user input */
+	printf("Low PWM set: %d\n"
-	
+	       "\n"
-
+	       "Hit ENTER when finished\n"
-	printf("Low PWM set, hit ENTER when finished\n"
+	       "\n", pwm_disarmed);
-		"\n");
 
 	while (1) {
 
-		/* Then set low PWM */
+		/* set disarmed PWM */
 		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
 			if (channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), 900);
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed);
+
 				if (ret != OK)
 					err(1, "PWM_SERVO_SET(%d)", i);
 			}
 		}
 
 		ret = poll(&fds, 1, 0);
+
 		if (ret > 0) {
 
 			read(0, &c, 1);
+
 			if (c == 13) {
 
 				break;
+
 			} else if (c == 0x03 || c == 0x63 || c == 'q') {
 				printf("ESC calibration exited\n");
 				exit(0);
 			}
 		}
+
 		/* rate limit to ~ 20 Hz */
 		usleep(50000);
 	}
 
-	
 	printf("ESC calibration finished\n");
 
 	exit(0);