Exemplo n.º 1
0
int
pca9685_main(int argc, char *argv[])
{
	int i2cdevice = -1;
	int i2caddr = ADDR; // 7bit

	int ch;

	// jump over start/off/etc and look at options first
	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
		switch (ch) {
		case 'a':
			i2caddr = strtol(optarg, NULL, 0);
			break;

		case 'b':
			i2cdevice = strtol(optarg, NULL, 0);
			break;

		default:
			pca9685_usage();
			exit(0);
		}
	}

	if (optind >= argc) {
		pca9685_usage();
		exit(1);
	}

	const char *verb = argv[optind];

	int fd;
	int ret;

	if (!strcmp(verb, "start")) {
		if (g_pca9685 != nullptr) {
			errx(1, "already started");
		}

		if (i2cdevice == -1) {
			// try the external bus first
			i2cdevice = PX4_I2C_BUS_EXPANSION;
			g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);

			if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
				delete g_pca9685;
				g_pca9685 = nullptr;
			}

			if (g_pca9685 == nullptr) {
				errx(1, "init failed");
			}
		}

		if (g_pca9685 == nullptr) {
			g_pca9685 = new PCA9685(i2cdevice, i2caddr);

			if (g_pca9685 == nullptr) {
				errx(1, "new failed");
			}

			if (OK != g_pca9685->init()) {
				delete g_pca9685;
				g_pca9685 = nullptr;
				errx(1, "init failed");
			}
		}
		fd = open(PCA9685_DEVICE_PATH, 0);
		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}
		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
		close(fd);


		exit(0);
	}

	// need the driver past this point
	if (g_pca9685 == nullptr) {
		warnx("not started, run pca9685 start");
		exit(1);
	}

	if (!strcmp(verb, "info")) {
		g_pca9685->info();
		exit(0);
	}

	if (!strcmp(verb, "reset")) {
		g_pca9685->reset();
		exit(0);
	}


	if (!strcmp(verb, "test")) {
		fd = open(PCA9685_DEVICE_PATH, 0);

		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}

		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);

		close(fd);
		exit(ret);
	}

	if (!strcmp(verb, "stop")) {
		fd = open(PCA9685_DEVICE_PATH, 0);

		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}

		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
		close(fd);

		// wait until we're not running any more
		for (unsigned i = 0; i < 15; i++) {
			if (!g_pca9685->is_running()) {
				break;
			}

			usleep(50000);
			printf(".");
			fflush(stdout);
		}
		printf("\n");
		fflush(stdout);

		if (!g_pca9685->is_running()) {
			delete g_pca9685;
			g_pca9685= nullptr;
			warnx("stopped, exiting");
			exit(0);
		} else {
			warnx("stop failed.");
			exit(1);
		}
	}

	pca9685_usage();
	exit(0);
}
Exemplo n.º 2
0
int
pca9685_main(int argc, char *argv[])
{
	int i2cdevice = -1;
	int i2caddr = ADDR; // 7bit

	int ch;

	// jump over start/off/etc and look at options first
	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
		switch (ch) {
		case 'a':
			i2caddr = strtol(optarg, NULL, 0);
			break;

		case 'b':
			i2cdevice = strtol(optarg, NULL, 0);
			break;

		default:
			pca9685_usage();
			exit(0);
		}
	}

	if (optind >= argc) {
		pca9685_usage();
		exit(1);
	}

	const char *verb = argv[optind];

	int fd;
	int ret;

	if (!strcmp(verb, "start")) {
		if (g_pca9685 != nullptr) {
			errx(1, "already started");
		}

		if (i2cdevice == -1) {
			// try the external bus first
			i2cdevice = PX4_I2C_BUS_EXPANSION;
			g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);

			if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
				delete g_pca9685;
				g_pca9685 = nullptr;
			}

			if (g_pca9685 == nullptr) {
				errx(1, "init failed");
			}
		}

		if (g_pca9685 == nullptr) {
			g_pca9685 = new PCA9685(i2cdevice, i2caddr);

			if (g_pca9685 == nullptr) {
				errx(1, "new failed");
			}

			if (OK != g_pca9685->init()) {
				delete g_pca9685;
				g_pca9685 = nullptr;
				errx(1, "init failed");
			}
		}

		fd = open(PCA9685_DEVICE_PATH, 0);

		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}

		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
		close(fd);


		exit(0);
	}

	// need the driver past this point
	if (g_pca9685 == nullptr) {
		warnx("not started, run pca9685 start");
		exit(1);
	}

	if (!strcmp(verb, "info")) {
		g_pca9685->info();
		exit(0);
	}

	if (!strcmp(verb, "reset")) {
		g_pca9685->reset();
		exit(0);
	}

	if (!strcmp(verb, "status")) {
		if (g_pca9685 != nullptr) {
			int i;
			struct actuator_controls_s actuation;
			uint16_t servoVals[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];

			if (g_pca9685->MixerInit()) {
				printf("Mixer initialized.\n");
			} else {
				printf("Mixer not initialized.\n");
			}

			g_pca9685->getActuation(&actuation, 1);
			printf("Actuator Group 1 Status\n");
			for (i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; ++i) {
				double val = actuation.control[i];
				printf("Act %d: %2.6f\n", i, val);
			}

			printf("\n");
			g_pca9685->getMotors((uint16_t*)&servoVals);
			printf("Raw Servos\n");
			for (i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) {
				uint16_t val = servoVals[i];
				printf("Servo %d: %d\n", i, val);
			}

			exit(0);
		} else {
			warnx("PCA9685 isn't running.");
			exit(1);
		}
	}


	if (!strcmp(verb, "test")) {
		fd = open(PCA9685_DEVICE_PATH, 0);

		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}

		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);

		close(fd);
		exit(ret);
	}

	if (!strcmp(verb, "stop")) {
		fd = open(PCA9685_DEVICE_PATH, 0);

		if (fd == -1) {
			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
		}

		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
		close(fd);

		// wait until we're not running any more
		for (unsigned i = 0; i < 15; i++) {
			if (!g_pca9685->is_running()) {
				break;
			}

			usleep(50000);
			printf(".");
			fflush(stdout);
		}

		printf("\n");
		fflush(stdout);

		if (!g_pca9685->is_running()) {
			delete g_pca9685;
			g_pca9685 = nullptr;
			warnx("stopped, exiting");
			exit(0);

		} else {
			warnx("stop failed.");
			exit(1);
		}
	}

	pca9685_usage();
	exit(0);
}