C++ orb_advertise_multi函数代码示例

bool create_pubs()
{
	// initialize the reports
	memset(&_gyro, 0, sizeof(struct gyro_report));
	memset(&_accel, 0, sizeof(struct accel_report));
	memset(&_mag, 0, sizeof(struct mag_report));

	_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
					&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);

	if (_gyro_pub == nullptr) {
		PX4_ERR("sensor_gyro advert fail");
		return false;
	}

	_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
					 &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);

	if (_accel_pub == nullptr) {
		PX4_ERR("sensor_accel advert fail");
		return false;
	}

	_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
				       &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);

	if (_mag_pub == nullptr) {
		PX4_ERR("sensor_mag advert fail");
		return false;
	}

	return true;
}

void PublicationBase::update(void *data)
{
	if (_handle != nullptr) {
		int ret = orb_publish(getMeta(), getHandle(), data);

		if (ret != PX4_OK) { warnx("publish fail"); }

	} else {
		orb_advert_t handle;

		if (_priority > 0) {
			handle = orb_advertise_multi(
					 getMeta(), data,
					 &_instance, _priority);

		} else {
			handle = orb_advertise(getMeta(), data);
		}

		if (int64_t(handle) != PX4_ERROR) {
			setHandle(handle);

		} else {
			warnx("advert fail");
		}
	}
}

int LidarLitePWM::init()
{
	/* do regular cdev init */
	int ret = CDev::init();

	if (ret != OK) {
		return ERROR;
	}

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

	if (_reports == nullptr) {
		return ERROR;
	}

	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

	if (_class_instance == CLASS_DEVICE_PRIMARY) {
		/* get a publish handle on the distance_sensor topic */
		struct distance_sensor_s ds_report;
		measure();
		_reports->get(&ds_report);
		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
							     &_orb_class_instance, ORB_PRIO_LOW);

		if (_distance_sensor_topic == nullptr) {
			debug("failed to create distance_sensor object. Did you start uORB?");
		}
	}

	return OK;
}

int uORBTest::UnitTest::test_unadvertise()
{
	test_note("Testing unadvertise");

	//we still have the advertisements from the previous test_multi calls.
	for (int i = 0; i < 4; ++i) {
		int ret = orb_unadvertise(_pfd[i]);

		if (ret != PX4_OK) {
			return test_fail("orb_unadvertise failed (%i)", ret);
		}
	}

	//try to advertise and see whether we get the right instance
	int instance_test[4];
	struct orb_test t;

	for (int i = 0; i < 4; ++i) {
		_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);

		if (instance_test[i] != i) {
			return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
		}
	}

	for (int i = 0; i < 4; ++i) {
		orb_unadvertise(_pfd[i]);
	}

	return test_note("PASS unadvertise");
}

int LidarLiteI2C::init()
{
	int ret = PX4_ERROR;

	/* do I2C init (and probe) first */
	if (I2C::init() != OK) {
		return ret;
	}

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

	if (_reports == nullptr) {
		return ret;
	}

	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

	/* get a publish handle on the range finder topic */
	struct distance_sensor_s ds_report = {};
	measure();
	_reports->get(&ds_report);
	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
				 &_orb_class_instance, ORB_PRIO_LOW);

	if (_distance_sensor_topic == nullptr) {
		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
	}

	ret = OK;
	/* sensor is ok, but we don't really know if it is within range */
	_sensor_ok = true;

	return ret;
}

int
BMI055_gyro::init()
{
	int ret;

	/* do SPI init (and probe) first */
	ret = SPI::init();

	/* if probe/setup failed, bail now */
	if (ret != OK) {
		DEVICE_DEBUG("SPI setup failed");
		return ret;
	}

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

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

	if (reset() != OK) {
		goto out;
	}

	/* Initialize offsets and scales */
	_gyro_scale.x_offset = 0;
	_gyro_scale.x_scale  = 1.0f;
	_gyro_scale.y_offset = 0;
	_gyro_scale.y_scale  = 1.0f;
	_gyro_scale.z_offset = 0;
	_gyro_scale.z_scale  = 1.0f;


	/* if probe/setup failed, bail now */
	if (ret != OK) {
		DEVICE_DEBUG("gyro init failed");
		return ret;
	}

	_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);

	measure();

	/* advertise sensor topic, measure manually to initialize valid report */
	struct gyro_report grp;
	_gyro_reports->get(&grp);

	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
					  &_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);

	if (_gyro_topic == nullptr) {
		warnx("ADVERT FAIL");
	}

out:
	return ret;
}

int
PX4FLOW::init()
{
	int ret = PX4_ERROR;

	/* do I2C init (and probe) first */
	if (I2C::init() != OK) {
		return ret;
	}

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

	if (_reports == nullptr) {
		return ret;
	}

	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

	/* get a publish handle on the range finder topic */
	struct distance_sensor_s ds_report = {};

	if (_class_instance == CLASS_DEVICE_PRIMARY) {
		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
					 &_orb_class_instance, ORB_PRIO_HIGH);

		if (_distance_sensor_topic == nullptr) {
			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
		}

	} else {
		DEVICE_LOG("not primary range device, not advertising");
	}

	ret = OK;
	/* sensor is ok, but we don't really know if it is within range */
	_sensor_ok = true;

	/* get yaw rotation from sensor frame to body frame */
	param_t rot = param_find("SENS_FLOW_ROT");

	/* only set it if the parameter exists */
	if (rot != PARAM_INVALID) {
		int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
		param_get(rot, &val);

		_sensor_rotation = (enum Rotation)val;
	}

	return ret;
}

int uORBTest::UnitTest::pub_test_multi2_main()
{
	int data_next_idx = 0;
	const int num_instances = 3;
	orb_advert_t orb_pub[num_instances];
	struct orb_test_medium data_topic;

	for (int i = 0; i < num_instances; ++i) {
		orb_advert_t &pub = orb_pub[i];
		int idx = i;
//		PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time());
		pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT);

		if (idx != i) {
			_thread_should_exit = true;
			PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx);
			return -1;
		}
	}

	usleep(100 * 1000);

	int message_counter = 0, num_messages = 50 * num_instances;

	while (message_counter++ < num_messages) {
		usleep(2); //make sure the timestamps are different
		orb_advert_t &pub = orb_pub[data_next_idx];

		data_topic.time = hrt_absolute_time();
		data_topic.val = data_next_idx;

		orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
//		PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time);

		data_next_idx = (data_next_idx + 1) % num_instances;

		if (data_next_idx == 0) {
			usleep(50 * 1000);
		}
	}

	usleep(100 * 1000);
	_thread_should_exit = true;

	for (int i = 0; i < num_instances; ++i) {
		orb_unadvertise(orb_pub[i]);
	}

	return 0;
}

int
FXAS21002C::init()
{
	/* do SPI init (and probe) first */
	if (SPI::init() != OK) {
		PX4_ERR("SPI init failed");
		return PX4_ERROR;
	}

	param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
	float gyro_cut = FXAS21002C_DEFAULT_FILTER_FREQ;

	if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
		PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));

		set_sw_lowpass_filter(FXAS21002C_DEFAULT_RATE, gyro_cut);

	} else {
		PX4_ERR("IMU_GYRO_CUTOFF param invalid");
	}

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

	if (_reports == nullptr) {
		return PX4_ERROR;
	}

	reset();

	/* fill report structures */
	measure();

	_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);

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

	/* measurement will have generated a report, publish */
	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
					  &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

	if (_gyro_topic == nullptr) {
		PX4_ERR("ADVERT ERR");
		return PX4_ERROR;
	}

	return PX4_OK;
}

int PX4Magnetometer::init()
{
	mag_report report{};
	report.device_id = _device_id.devid;

	if (_topic == nullptr) {
		_topic = orb_advertise_multi(ORB_ID(sensor_mag), &report, &_orb_class_instance, ORB_PRIO_HIGH - 1);

		if (_topic == nullptr) {
			PX4_ERR("Advertise failed.");
			return PX4_ERROR;
		}
	}

	return PX4_OK;
}

int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance,
		     int priority)
{
	if (*handle == nullptr) {
		*handle = orb_advertise_multi(meta, data, instance, priority);

		if (*handle != nullptr) {
			return 0;
		}

	} else {
		return orb_publish(meta, *handle, data);
	}

	return -1;
}

int
IIS328DQ::init()
{
	int ret = ERROR;
	
	if (probe() != OK)
		goto out;

	/* do SPI init (and probe) first */
	if (CDev::init() != OK)
		goto out;

	/* allocate basic report buffers */
	if (_reports != NULL) {
        ringbuf_deinit(_reports);
		vPortFree(_reports);
		_reports = NULL;
	}
	/* allocate basic report buffers */
	_reports = (ringbuf_t *) pvPortMalloc (sizeof(ringbuf_t));
	ringbuf_init(_reports, 2, sizeof(accel_report));

	if (_reports == NULL)
		goto out;

	_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);

	reset();

	measure();

	/* advertise sensor topic, measure manually to initialize valid report */
	struct accel_report arp;
	ringbuf_get(_reports, &arp, sizeof(arp));

	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
		&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

	if (_accel_topic == NULL) {
		DEVICE_DEBUG("failed to create sensor_accel publication");
	}

	ret = OK;
out:
	return ret;
}

void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) {
	actuator_outputs_s outputs;
	outputs.noutputs = numvalues;
	outputs.timestamp = hrt_absolute_time();

	for (size_t i = 0; i < _max_actuators; ++i) {
		outputs.output[i] = i < numvalues ? (float)values[i] : 0;
	}

	if (_outputs_pub == nullptr) {
		int instance = -1;
		_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
	} else {
		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
	}
}

int
SF0X::init()
{
	/* status */
	int ret = 0;

	do { /* create a scope to handle exit conditions using break */

		/* do regular cdev init */
		ret = CDev::init();

		if (ret != OK) { break; }

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

		if (_reports == nullptr) {
			warnx("mem err");
			ret = -1;
			break;
		}

		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

		if (_class_instance == CLASS_DEVICE_PRIMARY) {
			/* get a publish handle on the range finder topic */
			struct distance_sensor_s ds_report = {};

			_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
						 &_orb_class_instance, ORB_PRIO_HIGH);

			if (_distance_sensor_topic == nullptr) {
				DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
			}
		}

	} while (0);

	/* close the fd */
	::close(_fd);
	_fd = -1;

	return OK;
}

int
FXAS21002C::init()
{
	int ret = PX4_ERROR;

	/* do SPI init (and probe) first */
	if (SPI::init() != OK) {
		PX4_ERR("SPI init failed");
		goto out;
	}

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

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

	reset();

	/* fill report structures */
	measure();

	_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);

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

	/* measurement will have generated a report, publish */
	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
					  &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

	if (_gyro_topic == nullptr) {
		PX4_ERR("ADVERT ERR");
	}

	ret = OK;

out:
	return ret;
}