C++ px4_close函数代码示例

bool VtolType::init()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

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

	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);


	if (ret != PX4_OK) {
		PX4_ERR("failed getting max values");
		px4_close(fd);
		return false;
	}


	ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);

	if (ret != PX4_OK) {
		PX4_ERR("failed getting disarmed values");
		px4_close(fd);
		return false;
	}

	return 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;
	}
}

void led_deinit()
{
	if (leds >= 0) {
		px4_close(leds);
	}

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

int uORB::Manager::node_advertise
(
    const struct orb_metadata *meta,
    int *instance,
    int priority
)
{
  int fd = -1;
  int ret = ERROR;

  /* fill advertiser data */
  const struct orb_advertdata adv = { meta, instance, priority };

  /* open the control device */
  fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);

  if (fd < 0)
    goto out;

  /* advertise the object */
  ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);

  /* it's PX4_OK if it already exists */
  if ((PX4_OK != ret) && (EEXIST == errno)) {
    ret = PX4_OK;
  }

out:

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

  return ret;
}

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

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

	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_MOTOR_OFF;
		pwm_values.channel_count++;
	}

	int 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);
}

orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
{
  int result, fd;
  orb_advert_t advertiser;

  //warnx("orb_advertise_multi meta = %p\n", meta);

  /* open the node as an advertiser */
  fd = node_open(PUBSUB, meta, data, true, instance, priority);
  if (fd == ERROR) {
    warnx("node_open as advertiser failed.");
    return nullptr;
  }

  /* get the advertiser handle and close the node */
  result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
  px4_close(fd);
  if (result == ERROR) {
    warnx("px4_ioctl ORBIOCGADVERTISER  failed. fd = %d", fd);
    return nullptr;
  }

  /* the advertiser must perform an initial publish to initialise the object */
  result = orb_publish(meta, advertiser, data);
  if (result == ERROR) {
    warnx("orb_publish failed");
    return nullptr;
  }

  return advertiser;
}

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;
}

static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
	bool success = true;

	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));

	//Wait up to 2000ms to allow the driver to detect a GNSS receiver module
	px4_pollfd_struct_t fds[1];
	fds[0].fd = gpsSub;
	fds[0].events = POLLIN;
	if(px4_poll(fds, 1, 2000) <= 0) {
		success = false;
	}
	else {
		struct vehicle_gps_position_s gps;
		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
			success = false;
		}
	}

	//Report failure to detect module
	if (!success) {
		if (report_fail) {
			mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
		}
	}

	px4_close(gpsSub);
	return success;
}

/**
* 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);
}

/**
 * Start the driver on a specific bus.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
int
start_bus(uint8_t rotation, int i2c_bus)
{
	int fd = -1;

	if (g_dev != nullptr) {
		PX4_ERR("already started");
		return PX4_ERROR;
	}

	/* create the driver */
	g_dev = new SF1XX(rotation, i2c_bus);

	if (g_dev == nullptr) {
		goto fail;
	}

	if (OK != g_dev->init()) {
		goto fail;
	}

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

	if (fd < 0) {
		goto fail;
	}

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

	px4_close(fd);
	return PX4_OK;

fail:

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

	return PX4_ERROR;
}

// 1M 8N1 serial connection to NRF51
int
Syslink::open_serial(const char *dev)
{
#ifndef B1000000
#define B1000000 1000000
#endif

	int rate = B1000000;

	// open uart
	int fd = px4_open(dev, O_RDWR | O_NOCTTY);
	int termios_state = -1;

	if (fd < 0) {
		PX4_ERR("failed to open uart device!");
		return -1;
	}

	// set baud rate
	struct termios config;
	tcgetattr(fd, &config);

	// clear ONLCR flag (which appends a CR for every LF)
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);

	// Disable hardware flow control
	config.c_cflag &= ~CRTSCTS;


	/* Set baud rate */
	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
		warnx("ERR SET BAUD %s: %d\n", dev, termios_state);
		px4_close(fd);
		return -1;
	}

	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
		PX4_WARN("ERR SET CONF %s\n", dev);
		px4_close(fd);
		return -1;
	}

	return fd;
}

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;
}

int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
{
	/*
	 * Generate the path to the node and try to open it.
	 */
	char path[orb_maxpath];
	int inst = instance;
	int ret = uORB::Utils::node_mkpath(path, meta, &inst);

	if (ret != OK) {
		errno = -ret;
		return PX4_ERROR;
	}

#if defined(__PX4_NUTTX)
	struct stat buffer;
	ret = stat(path, &buffer);
#else
	ret = px4_access(path, F_OK);

#ifdef ORB_COMMUNICATOR

	if (ret == -1 && meta != nullptr && !_remote_topics.empty()) {
		ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR;
	}

#endif /* ORB_COMMUNICATOR */

#endif

	if (ret == 0) {
		// we know the topic exists, but it's not necessarily advertised/published yet (for example
		// if there is only a subscriber)
		// The open() will not lead to memory allocations.
		int fd = px4_open(path, 0);

		if (fd >= 0) {
			unsigned long is_published;

			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
				if (!is_published) {
					ret = PX4_ERROR;
				}
			}

			px4_close(fd);
		}
	}

	return ret;
}

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);
}

static int
accel(int argc, char *argv[], const char *path)
{
	printf("\tACCEL: test start\n");
	fflush(stdout);

	int		fd;
	struct accel_report buf;
	int		ret;

	fd = px4_open(path, O_RDONLY);

	if (fd < 0) {
		printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n");
		return ERROR;
	}

	/* wait at least 100ms, sensor should have data after no more than 20ms */
	usleep(100000);

	/* read data - expect samples */
	ret = px4_read(fd, &buf, sizeof(buf));

	if (ret != sizeof(buf)) {
		printf("\tACCEL: read1 fail (%d)\n", ret);
		return ERROR;

	} else {
		printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
	}

	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
		warnx("ACCEL acceleration values out of range!");
		return ERROR;
	}

	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);

	if (len < 8.0f || len > 12.0f) {
		warnx("ACCEL scale error!");
		return ERROR;
	}

	/* Let user know everything is ok */
	printf("\tOK: ACCEL passed all tests successfully\n");
	px4_close(fd);

	return OK;
}