C++ px4_ioctl函数代码示例

/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(unsigned pwm_value)
{
	int ret;
	unsigned servo_count;
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count = _params->vtol_motor_count;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting max values");
	}

	px4_close(fd);
}

/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	unsigned servo_count;
	int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	unsigned pwm_value = _params->idle_pwm_mc;
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count++;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);

	flag_idle_mc = true;
}

/**
 * @brief Resets the driver.
 */
void
reset()
{
	if (!instance) {
		PX4_WARN("No ll40ls driver running");
		return;
	}

	int fd = px4_open(instance->get_dev_name(), O_RDONLY);

	if (fd < 0) {
		PX4_ERR("Error opening fd");
		return;
	}

	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
		PX4_ERR("driver reset failed");
		px4_close(fd);
		return;
	}

	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		PX4_ERR("driver poll restart failed");
		px4_close(fd);
		return;
	}
}

bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
		return false;
	}

	int ret;

	if (type == pwm_limit_type::TYPE_MINIMUM) {
		ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	} else {
		ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
	}

	px4_close(fd);


	if (ret != OK) {
		PX4_ERR("failed setting max values");
		return false;
	}

	return true;
}

static int
load(const char *devname, const char *fname)
{
	// sleep a while to ensure device has been set up
	usleep(20000);
	
	int		dev;
	char		buf[2048];

	/* open the device */
	if ((dev = px4_open(devname, 0)) < 0) {
		warnx("can't open %s\n", devname);
		return 1;
	}

	/* reset mixers on the device */
	if (px4_ioctl(dev, MIXERIOCRESET, 0)) {
		warnx("can't reset mixers on %s", devname);
		return 1;
	}

	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
		warnx("can't load mixer: %s", fname);
		return 1;
	}

	/* XXX pass the buffer to the device */
	int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);

	if (ret < 0) {
		warnx("error loading mixers from %s", fname);
		return 1;
	}
	return 0;
}

int test_gpio(int argc, char *argv[])
{
	int		ret = 0;

#ifdef PX4IO_DEVICE_PATH

	int fd = px4_open(PX4IO_DEVICE_PATH, 0);

	if (fd < 0) {
		printf("GPIO: open fail\n");
		return ERROR;
	}

	/* set all GPIOs to default state */
	px4_ioctl(fd, GPIO_RESET, ~0);


	/* XXX need to add some GPIO waving stuff here */


	/* Go back to default */
	px4_ioctl(fd, GPIO_RESET, ~0);

	px4_close(fd);
	printf("\t GPIO test successful.\n");

#endif


	return ret;
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct differential_pressure_s report;
	ssize_t sz;
	int ret;

	int fd = px4_open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		PX4_ERR("%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
	}

	/* do a simple demand read */
	sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		PX4_ERR("immediate read failed");
	}

	PX4_INFO("single read");
	PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		PX4_ERR("failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			PX4_ERR("timed out waiting for sensor data");
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		PX4_INFO("periodic read %u", i);
		PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
	}

	/* reset the sensor polling to its default rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		PX4_ERR("failed to set default rate");
	}

	errx(0, "PASS");
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct distance_sensor_s report;
	ssize_t sz;

	int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
	}

	/* do a simple demand read */
	sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	print_message(report);

	/* start the sensor polling at 2 Hz rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		errx(1, "failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		px4_pollfd_struct_t fds{};

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		int ret = px4_poll(&fds, 1, 2000);

		if (ret != 1) {
			warnx("timed out");
			break;
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
			break;
		}

		print_message(report);
	}

	/* reset the sensor polling to the default rate */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		errx(1, "ERR: DEF RATE");
	}

	errx(0, "PASS");
}

// perform some basic functional tests on the driver;
// make sure we can collect data from the sensor in polled
// and automatic modes.
void test()
{
	int fd = px4_open(PATH_MS5525, O_RDONLY);

	if (fd < 0) {
		PX4_WARN("%s open failed (try 'ms5525_airspeed start' if the driver is not running", PATH_MS5525);
		return;
	}

	// do a simple demand read
	differential_pressure_s report;
	ssize_t sz = px4_read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		PX4_WARN("immediate read failed");
		return;
	}

	PX4_WARN("single read");
	PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		PX4_WARN("failed to set 2Hz poll rate");
		return;
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		px4_pollfd_struct_t fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		int ret = px4_poll(&fds, 1, 2000);

		if (ret != 1) {
			PX4_ERR("timed out");
		}

		/* now go get it */
		sz = px4_read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			PX4_ERR("periodic read failed");
		}

		PX4_WARN("periodic read %u", i);
		PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
		PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
	}

	/* reset the sensor polling to its default rate */
	if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		PX4_WARN("failed to set default rate");
	}
}

int test_tone(int argc, char *argv[])
{
	int fd, result;
	unsigned long tone;

	fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);

	if (fd < 0) {
		printf("failed opening " TONEALARM0_DEVICE_PATH "\n");
		goto out;
	}

	tone = 1;

	if (argc == 2) {
		tone = atoi(argv[1]);
	}

	if (tone  == 0) {
		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);

		if (result < 0) {
			printf("failed clearing alarms\n");
			goto out;

		} else {
			printf("Alarm stopped.\n");
		}

	} else {
		result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE);

		if (result < 0) {
			printf("failed clearing alarms\n");
			goto out;
		}

		result = px4_ioctl(fd, TONE_SET_ALARM, tone);

		if (result < 0) {
			printf("failed setting alarm %lu\n", tone);

		} else {
			printf("Alarm %lu (disable with: tests tone 0)\n", tone);
		}
	}

out:

	if (fd >= 0) {
		px4_close(fd);
	}

	return 0;
}

void Tiltrotor::set_rear_motor_state(rear_motor_state state)
{
	int pwm_value = PWM_DEFAULT_MAX;

	// map desired rear rotor state to max allowed pwm signal
	switch (state) {
	case ENABLED:
		pwm_value = PWM_DEFAULT_MAX;
		_rear_motors = ENABLED;
		break;

	case DISABLED:
		pwm_value = PWM_LOWEST_MAX;
		_rear_motors = DISABLED;
		break;

	case IDLE:
		pwm_value = _params->idle_pwm_mc;
		_rear_motors = IDLE;
		break;
	}

	int ret;
	unsigned servo_count;
	char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {PX4_WARN("can't open %s", dev);}

	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		if (is_motor_off_channel(i)) {
			pwm_values.values[i] = pwm_value;

		} else {
			pwm_values.values[i] = PWM_DEFAULT_MAX;
		}

		pwm_values.channel_count = _params->vtol_motor_count;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {PX4_WARN("failed setting max values");}

	px4_close(fd);
}

int led_off(int led)
{
	if (leds < 0) {
		return leds;
	}
	return px4_ioctl(leds, LED_OFF, led);
}

// Start the driver.
// This function call only returns once the driver is up and running
// or failed to detect the sensor.
void
start(uint8_t i2c_bus)
{
	int fd = -1;

	if (g_dev != nullptr) {
		PX4_ERR("already started");
		goto fail;
	}

	g_dev = new MS5525(i2c_bus, I2C_ADDRESS_1_MS5525DSO, PATH_MS5525);

	/* check if the MS4525DO was instantiated */
	if (g_dev == nullptr) {
		goto fail;
	}

	/* try the next MS5525DSO if init fails */
	if (OK != g_dev->Airspeed::init()) {
		delete g_dev;

		PX4_WARN("trying MS5525 address 2");

		g_dev = new MS5525(i2c_bus, I2C_ADDRESS_2_MS5525DSO, PATH_MS5525);

		/* check if the MS5525DSO was instantiated */
		if (g_dev == nullptr) {
			PX4_WARN("MS5525 was not instantiated");
			goto fail;
		}

		/* both versions failed if the init for the MS5525DSO fails, give up */
		if (OK != g_dev->Airspeed::init()) {
			PX4_WARN("MS5525 init fail");
			goto fail;
		}
	}

	/* set the poll rate to default, starts automatic data collection */
	fd = px4_open(PATH_MS5525, O_RDONLY);

	if (fd < 0) {
		goto fail;
	}

	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		goto fail;
	}

	return;

fail:

	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;
	}

	PX4_WARN("no MS5525 airspeed sensor connected");
}

int led_on(int led)
{
	if (leds < 0) {
		return leds;
	}
	return px4_ioctl(leds, LED_ON, led);
}

int led_toggle(int led)
{
	if (leds < 0) {
		return leds;
	}
	return px4_ioctl(leds, LED_TOGGLE, led);
}