C++ PX4_WARN函数代码示例

int px4_prctl(int option, const char *arg2, unsigned pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name - Not supported
		// rv = pthread_setname_np(pthread_self(), arg2);
		rv = -1;
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}

int Ekf2::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("ekf2",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   9000,
					   (px4_main_t)&Ekf2::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		PX4_WARN("task start failed");
		return -errno;
	}

	return OK;
}

/**
 * Start the driver.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
void
start(int i2c_bus)
{
	int fd;

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

	/* create the driver */
	g_dev = new ETSAirspeed(i2c_bus);

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

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

	/* set the poll rate to default, starts automatic data collection */
	fd = px4_open(AIRSPEED0_DEVICE_PATH, 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 ETS airspeed sensor connected");
}

int
AirspeedSim::init()
{
	int ret = ERROR;

	/* init base class */
	if (CDev::init() != OK) {
		DEVICE_DEBUG("CDev init failed");
		goto out;
	}

	/* allocate basic report buffers */
	_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));

	if (_reports == nullptr) {
		goto out;
	}

	/* register alternate interfaces if we have to */
	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);

	/* publication init */
	if (_class_instance == CLASS_DEVICE_PRIMARY) {

		/* advertise sensor topic, measure manually to initialize valid report */
		struct differential_pressure_s arp;
		measure();
		_reports->get(&arp);

		/* measurement will have generated a report, publish */
		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);

		if (_airspeed_pub == nullptr) {
			PX4_WARN("uORB started?");
		}
	}

	ret = OK;

out:
	return ret;
}

/**
 * Start the driver.
 */
void
start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
{
	if (g_dev[gps_num - 1] != nullptr) {
		PX4_WARN("GPS %i already started", gps_num);
		return;
	}

	/* create the driver */
	g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);

	if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) {
		if (g_dev[gps_num - 1] != nullptr) {
			delete g_dev[gps_num - 1];
			g_dev[gps_num - 1] = nullptr;
		}

		PX4_ERR("start of GPS %i failed", gps_num);
	}
}

int RcInput::start()
{
	int result = 0;

	result = navio_rc_init();

	if (result != 0) {
		PX4_WARN("error: RC initialization failed");
		return -1;
	}

	_isRunning = true;
	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);

	if (result == -1) {
		_isRunning = false;
	}

	return result;
}

Device::Device(const char *name) :
	// public
	// protected
	_name(name),
	_debug_enabled(false)
{
	int ret = px4_sem_init(&_lock, 0, 1);

	if (ret != 0) {
		PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno));
	}

	/* setup a default device ID. When bus_type is UNKNOWN the
	   other fields are invalid */
	_device_id.devid = 0;
	_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
	_device_id.devid_s.bus = 0;
	_device_id.devid_s.address = 0;
	_device_id.devid_s.devtype = 0;
}

int
VtolAttitudeControl::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("vtol_att_control",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 10,
					   2048,
					   (px4_main_t)&VtolAttitudeControl::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		PX4_WARN("task start failed");
		return -errno;
	}

	return OK;
}

int TAP_ESC::send_packet(EscPacket &packet, int responder)
{
	if (responder >= 0) {

		if (responder > _channels_count) {
			return -EINVAL;
		}

		select_responder(responder);
	}

	int packet_len = crc_packet(packet);
	int ret = ::write(_uart_fd, &packet.head, packet_len);

	if (ret != packet_len) {
		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
	}

	return ret;
}

/**
 * Automatic scale calibration.
 *
 * Basic idea:
 *
 *   output = (ext field +- 1.1 Ga self-test) * scale factor
 *
 * and consequently:
 *
 *   1.1 Ga = (excited - normal) * scale factor
 *   scale factor = (excited - normal) / 1.1 Ga
 *
 *   sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
 *   sz  = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
 *
 * By subtracting the non-excited measurement the pure 1.1 Ga reading
 * can be extracted and the sensitivity of all axes can be matched.
 *
 * SELF TEST OPERATION
 * To check the IST8310L for proper operation, a self test feature in incorporated
 * in which the sensor will change the polarity on all 3 axis. The values with and
 * with and without selftest on shoult be compared and if the absolete value are equal
 * the IC is functional.
 */
int calibrate(enum IST8310_BUS busid)
{
	int ret;
	struct ist8310_bus_option &bus = find_bus(busid);
	const char *path = bus.devpath;

	int fd = open(path, O_RDONLY);

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

	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
		PX4_WARN("failed to enable sensor calibration mode");
	}

	close(fd);

	return ret;
}

int micrortps_client_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage(argv[0]);
		return -1;
	}

	if (!strcmp(argv[1], "start")) {
		PX4_WARN("PX4 built without RTPS bridge support, EXITING...\n");
		return -1;
	}

	if (!strcmp(argv[1], "stop")) {
		PX4_INFO("Not running");
		return -1;
	}

	usage(argv[0]);

	return -1;
}

/**
 * Start the driver.
 */
void
start(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info,
      int gps_num)
{
	if (g_dev[gps_num - 1] != nullptr) {
		PX4_WARN("GPS %i already started", gps_num);
		return;
	}

	/* create the driver */
	g_dev[gps_num - 1] = new GPS(path, mode, interface, fake_gps, enable_sat_info, gps_num);

	if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) {
		if (g_dev[gps_num - 1] != nullptr) {
			delete g_dev[gps_num - 1];
			g_dev[gps_num - 1] = nullptr;
		}

		PX4_ERR("start of GPS %i failed", gps_num);
	}
}

int px4_prctl(int option, const char *arg2, px4_task_t pid)
{
	int rv;

	switch (option) {
	case PR_SET_NAME:
		// set the threads name
#ifdef __PX4_DARWIN
		rv = pthread_setname_np(arg2);
#else
		rv = pthread_setname_np(pthread_self(), arg2);
#endif
		break;

	default:
		rv = -1;
		PX4_WARN("FAILED SETTING TASK NAME");
		break;
	}

	return rv;
}

int initialise_uart()
{
	// open uart
	_uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
	int termios_state = -1;

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

	// set baud rate
	int speed = B250000;
	struct termios uart_config;
	tcgetattr(_uart_fd, &uart_config);

	// clear ONLCR flag (which appends a CR for every LF)
	uart_config.c_oflag &= ~ONLCR;

	// set baud rate
	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
		PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state);
		close(_uart_fd);
		return -1;
	}

	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
		PX4_ERR("tcsetattr failed for %s\n", _device);
		close(_uart_fd);
		return -1;
	}

	// setup output flow control
	if (enable_flow_control(false)) {
		PX4_WARN("hardware flow disable failed");
	}

	return _uart_fd;
}

void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
{

	uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM];
	memset(rpm, 0, sizeof(rpm));
	uint8_t motor_cnt = num_pwm;
	static uint8_t which_to_respone = 0;

	for (uint8_t i = 0; i < motor_cnt; i++) {
		rpm[i] = pwm[i];

		if (rpm[i] > RPMMAX) {
			rpm[i] = RPMMAX;

		} else if (rpm[i] < RPMSTOPPED) {
			rpm[i] = RPMSTOPPED;
		}
	}

	rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK);


	EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
	packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]);

	for (uint8_t i = 0; i < _channels_count; i++) {
		packet.d.reqRun.rpm_flags[i] = rpm[i];
	}

	int ret = send_packet(packet, which_to_respone);

	if (++which_to_respone == _channels_count) {
		which_to_respone = 0;
	}

	if (ret < 1) {
		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
	}
}