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

int
MPU9250_mag::init()
{
	int ret;

	ret = CDev::init();

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

	_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));

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

	_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);

	ak8963_setup();

	/* advertise sensor topic, measure manually to initialize valid report */
	struct mag_report mrp;
	_mag_reports->get(&mrp);

	_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
					 &_mag_orb_class_instance, ORB_PRIO_LOW);
//			   &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);

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

out:
	return ret;
}

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

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

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

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

	_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);

	reset();

	measure();

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

	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
					  &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

	if (_gyro_topic == nullptr) {
		DEVICE_DEBUG("failed to create sensor_gyro publication");
	}

	ret = OK;
out:
	return ret;
}

int
CM8JL65::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) {
			PX4_ERR("alloc failed");
			ret = -1;
			break;
		}

		_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 = {};

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

		if (_distance_sensor_topic == nullptr) {
			PX4_ERR("failed to create distance_sensor object");
		}

	} while (0);

	return ret;
}

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

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

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

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

    _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?");
        }
    }

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

//.........这里部分代码省略.........
		ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB,
				       raw_temperature, sizeof(raw_temperature));

		if (ret == OK) {
			int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) +
					 raw_temperature[0];
			new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
		}

	} else {
		new_report.temperature = _last_report.temperature;
	}


	/*
	 * RAW outputs
	 *
	 * to align the sensor axes with the board, x and y need to be flipped
	 * and y needs to be negated
	 */
	new_report.x_raw = -report.y;
	new_report.y_raw = report.x;
	/* z remains z */
	new_report.z_raw = report.z;

	/* scale values for output */

	// XXX revisit for SPI part, might require a bus type IOCTL
	unsigned dummy;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
	new_report.is_external = !sensor_is_onboard;

	if (sensor_is_onboard) {
		// convert onboard so it matches offboard for the
		// scaling below
		report.y = -report.y;
		report.x = -report.x;
	}

	/* the standard external mag by 3DR has x pointing to the
	 * right, y pointing backwards, and z down, therefore switch x
	 * and y and invert y */
	//TODO: sort out axes mapping
	xraw_f = -report.y;
	yraw_f = report.x;
	zraw_f = report.z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

	if (!(_pub_blocked)) {

		if (_mag_topic != nullptr) {
			/* publish it */
			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);

		} else {
			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
							 &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

			if (_mag_topic == nullptr) {
				DEVICE_DEBUG("ADVERT FAIL");
			}
		}
	}

	_last_report = new_report;

	/* post a report to the ring */
	_reports->force(&new_report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/*
	  periodically check the configuration
	  registers. With a bad I2C cable it is possible for the
	  registers to become corrupt, leading to bad readings. It
	  doesn't happen often, but given the poor cables some
	  vehicles have it is worth checking for.
	 */
	check_counter = perf_event_count(_sample_perf) % 256;


	if (check_counter == 0) {
		check_conf();
	}

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}