C++ ringbuffer::RingBuffer类代码示例

int
FXOS8700CQ::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 */
	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));

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

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

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

	reset();

	/* do CDev init for the mag device node */
	ret = _mag->init();

	if (ret != OK) {
		PX4_ERR("MAG init failed");
		goto out;
	}

	/* fill report structures */
	measure();

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

	/* measurement will have generated a report, publish */
	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
					       &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);

	if (_mag->_mag_topic == nullptr) {
		PX4_ERR("ADVERT ERR");
	}


	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);

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

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

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

out:
	return ret;
}

int
ACCELSIM::init()
{
	int ret = -1;

	struct mag_report mrp = {};
	struct accel_report arp = {};

	/* do SIM init first */
	if (VirtDevObj::init() != 0) {
		PX4_WARN("SIM init failed");
		goto out;
	}

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

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

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

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

	/* do init for the mag device node */
	ret = _mag->init();

	if (ret != OK) {
		PX4_WARN("MAG init failed");
		goto out;
	}

	/* fill report structures */
	_measure();

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

	/* measurement will have generated a report, publish */
	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
					       &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);

	if (_mag->_mag_topic == nullptr) {
		PX4_WARN("ADVERT ERR");
	}

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

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);

	if (_accel_topic == nullptr) {
		PX4_WARN("ADVERT ERR");
	}

out:
	return ret;
}

int
LIS3MDL::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t		x[2];
		uint8_t		y[2];
		uint8_t		z[2];
		uint8_t		t[2];
	}	lis_report;

	struct {
		int16_t		x;
		int16_t		y;
		int16_t		z;
		int16_t		t;
	} report;
#pragma pack(pop)

	int	ret;
//	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	bool sensor_is_onboard = false;

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.error_count = perf_event_count(_comms_errors);

	/*
	 * @note  We could read the status register here, which could tell us that
	 *        we were too early and that the output registers are still being
	 *        written.  In the common case that would just slow us down, and
	 *        we're better off just never being early.
	 */

	/* get measurements from the device */
	ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* convert the data we just received */
	report.x = (((int16_t)lis_report.x[1]) << 8) + lis_report.x[0];
	report.y = (((int16_t)lis_report.y[1]) << 8) + lis_report.y[0];
	report.z = (((int16_t)lis_report.z[1]) << 8) + lis_report.z[0];
	report.t = (((int16_t)lis_report.t[1]) << 8) + lis_report.t[0];

	/* get measurements from the device */
	new_report.temperature = report.t;
	new_report.temperature = 25 + (report.t / (16 * 8.0f));

	// XXX revisit for SPI part, might require a bus type IOCTL

	unsigned dummy;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);

	/*
	 * RAW outputs
	 *
	 */
	new_report.x_raw = report.x;
	new_report.y_raw = report.y;
	new_report.z_raw = report.z;

	/* the LIS3MDL mag on Pixhawk Pro by Drotek has x pointing towards,
	 * y pointing to the right, and z down, therefore no switch needed,
	 * it is better to have no artificial rotation inside the
	 * driver and then use the startup script with -R command with the
	 * real rotation between the sensor and body frame */
	xraw_f = report.x;
	yraw_f = report.y;
	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);
//.........这里部分代码省略.........

int
SRF02_I2C::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_cycling_rate);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();

					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					int ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_cycling_rate)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = px4_enter_critical_section();

			if (!_reports->resize(arg)) {
				px4_leave_critical_section(flags);
				return -ENOMEM;
			}

			px4_leave_critical_section(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	case RANGEFINDERIOCSETMINIUMDISTANCE: {
			set_minimum_distance(*(float *)arg);
			return 0;
//.........这里部分代码省略.........

ssize_t
BAROSIM::devRead(void *buffer, size_t buflen)
{
	unsigned count = buflen / sizeof(struct baro_report);
	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
	int ret = 0;

	/* buffer must be large enough */
	if (count < 1) {
		return -ENOSPC;
	}

	/* if automatic measurement is enabled */
	if (m_sample_interval_usecs > 0) {

		/*
		 * While there is space in the caller's buffer, and reports, copy them.
		 * Note that we may be pre-empted by the workq thread while we are doing this;
		 * we are careful to avoid racing with them.
		 */
		while (count--) {
			if (_reports->get(brp)) {
				ret += sizeof(*brp);
				brp++;
			}
		}

		/* if there was no data, warn the caller */
		return ret ? ret : -EAGAIN;
	}

	/* manual measurement - run one conversion */
	do {
		_measure_phase = 0;
		_reports->flush();

		/* do temperature first */
		if (OK != measure()) {
			ret = -EIO;
			break;
		}

		usleep(BAROSIM_CONVERSION_INTERVAL);

		if (OK != collect()) {
			ret = -EIO;
			break;
		}

		/* now do a pressure measurement */
		if (OK != measure()) {
			ret = -EIO;
			break;
		}

		usleep(BAROSIM_CONVERSION_INTERVAL);

		if (OK != collect()) {
			ret = -EIO;
			break;
		}

		/* state machine will have generated a report, copy it out */
		if (_reports->get(brp)) {
			ret = sizeof(*brp);
		}

	} while (0);

	return ret;
}

void
ACCELSIM::_measure()
{
#if 0
	static int x = 0;

	// Verify the samples are being taken at the expected rate
	if (x == 99) {
		x = 0;
		PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time());

	} else {
		x++;
	}

#endif

	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		status;
		float		temperature;
		float		x;
		float		y;
		float		z;
	} raw_accel_report;
#pragma pack(pop)

	accel_report accel_report = {};

	/* start the performance counter */
	perf_begin(_accel_sample_perf);

	/* fetch data from the sensor */
	memset(&raw_accel_report, 0, sizeof(raw_accel_report));
	raw_accel_report.cmd = DIR_READ | ACC_READ;

	if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) {
		return;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */


	accel_report.timestamp = hrt_absolute_time();

	// use the temperature from the last mag reading
	accel_report.temperature = _last_temperature;

	// report the error count as the sum of the number of bad
	// register reads and bad values. This allows the higher level
	// code to decide if it should use this sensor based on
	// whether it has had failures
	accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);

	accel_report.x_raw = (int16_t)(raw_accel_report.x / _accel_range_scale);
	accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
	accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);

	accel_report.x = raw_accel_report.x;
	accel_report.y = raw_accel_report.y;
	accel_report.z = raw_accel_report.z;

	accel_report.scaling = _accel_range_scale;
	accel_report.range_m_s2 = _accel_range_m_s2;

	_accel_reports->force(&accel_report);

	/* notify anyone waiting for data */
	DevMgr::updateNotify(*this);

	if (!(m_pub_blocked)) {
		/* publish it */

		// The first call to measure() is from init() and _accel_topic is not
		// yet initialized
		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}
	}

	_accel_read++;

	/* stop the perf counter */
	perf_end(_accel_sample_perf);
}

int
TFMINI::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_conversion_interval);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {

					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					int ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_conversion_interval)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			ATOMIC_ENTER;

			if (!_reports->resize(arg)) {
				ATOMIC_LEAVE;
				return -ENOMEM;
			}

			ATOMIC_LEAVE;

			return OK;
		}

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}

int
HMC5883::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t		x[2];
		uint8_t		z[2];
		uint8_t		y[2];
	}	hmc_report;
#pragma pack(pop)
	struct {
		int16_t		x, y, z;
	} report;

	int	ret;
	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	bool sensor_is_onboard = false;

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.error_count = perf_event_count(_comms_errors);
	new_report.range_ga = _range_ga;
	new_report.scaling = _range_scale;
	new_report.device_id = _device_id.devid;

	/*
	 * @note  We could read the status register here, which could tell us that
	 *        we were too early and that the output registers are still being
	 *        written.  In the common case that would just slow us down, and
	 *        we're better off just never being early.
	 */

	/* get measurements from the device */
	ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* swap the data we just received */
	report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
	report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
	report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];

	/*
	 * If any of the values are -4096, there was an internal math error in the sensor.
	 * Generalise this to a simple range check that will also catch some bit errors.
	 */
	if ((abs(report.x) > 2048) ||
	    (abs(report.y) > 2048) ||
	    (abs(report.z) > 2048)) {
		perf_count(_comms_errors);
		goto out;
	}

	/* get measurements from the device */
	new_report.temperature = 0;

	if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) {
		/*
		  if temperature compensation is enabled read the
		  temperature too.

		  We read the temperature every 10 samples to avoid
		  excessive I2C traffic
		 */
		if (_temperature_counter++ == 10) {
			uint8_t raw_temperature[2];

			_temperature_counter = 0;

			ret = _interface->read(ADDR_TEMP_OUT_MSB,
					       raw_temperature, sizeof(raw_temperature));

			if (ret == OK) {
				int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
						 raw_temperature[1];
				new_report.temperature = 25 + (temp16 / (16 * 8.0f));
				_temperature_error_count = 0;

			} else {
				_temperature_error_count++;

				if (_temperature_error_count == 10) {
					/*
					  it probably really is an old HMC5883,
					  and can't do temperature. Disable it
					*/
					_temperature_error_count = 0;
					DEVICE_DEBUG("disabling temperature compensation");
					set_temperature_compensation(0);
//.........这里部分代码省略.........

int
ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
	unsigned long ul_arg = (unsigned long)arg;

	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (ul_arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				m_sample_interval_usecs = 0;
				return OK;

			/* external signalling not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
				return devIOCTL(SENSORIOCSPOLLRATE, 1600);

			case SENSOR_POLLRATE_DEFAULT:
				return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE);

			/* adjust to a legal polling interval in Hz */
			default: {
					/* convert hz to hrt interval via microseconds */
					unsigned interval = 1000000 / ul_arg;

					/* check against maximum sane rate */
					if (interval < 500) {
						return -EINVAL;
					}

					/* adjust filters */
					accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq());

					bool want_start = (m_sample_interval_usecs == 0);

					/* update interval for next measurement */
					setSampleInterval(interval);

					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (m_sample_interval_usecs == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return 1000000 / m_sample_interval_usecs;

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((ul_arg < 1) || (ul_arg > 100)) {
				return -EINVAL;
			}

			if (!_accel_reports->resize(ul_arg)) {
				return -ENOMEM;
			}

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _accel_reports->size();

	case SENSORIOCRESET:
		// Nothing to do for simulator
		return OK;

	case ACCELIOCSSAMPLERATE:
		// No need to set internal sampling rate for simulator
		return OK;

	case ACCELIOCGSAMPLERATE:
		return _accel_samplerate;

	case ACCELIOCSLOWPASS: {
			return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)ul_arg);
		}

	case ACCELIOCSSCALE: {
			/* copy scale, but only if off by a few percent */
			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
			float sum = s->x_scale + s->y_scale + s->z_scale;

//.........这里部分代码省略.........

void
FXOS8700CQ::measure()
{
	/* status register and data as read back from the device */

#pragma pack(push, 1)
	struct {
		uint8_t		cmd[2];
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
		int16_t		mx;
		int16_t		my;
		int16_t		mz;
	} raw_accel_mag_report;
#pragma pack(pop)

	accel_report accel_report;

	/* start the performance counter */
	perf_begin(_accel_sample_perf);

	check_registers();

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again.
		_register_wait--;
		perf_end(_accel_sample_perf);
		return;
	}

	/* fetch data from the sensor */
	memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report));
	raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS);
	raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS);
	transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));

	if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) {
		perf_end(_accel_sample_perf);
		perf_count(_accel_duplicates);
		return;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	accel_report.timestamp = hrt_absolute_time();

	/*
	 * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
	 * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
	 * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
	 * temperature sensor is uncalibrated and its output for a given temperature will vary from
	 * one device to the next
	 */

	write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7));
	_last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f;
	write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));

	accel_report.temperature = _last_temperature;

	// report the error count as the sum of the number of bad
	// register reads and bad values. This allows the higher level
	// code to decide if it should use this sensor based on
	// whether it has had failures
	accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);

	accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x);
	accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
	accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);

	/* Save off the Mag readings todo: revist integrating theses */

	_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
	_last_raw_mag_y = swap16(raw_accel_mag_report.my);
	_last_raw_mag_z = swap16(raw_accel_mag_report.mz);

	float xraw_f = accel_report.x_raw;
	float yraw_f = accel_report.y_raw;
	float zraw_f = accel_report.z_raw;

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

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
//.........这里部分代码省略.........

void
FXOS8700CQ::mag_measure()
{
	/* status register and data as read back from the device */

	mag_report mag_report {};

	/* start the performance counter */
	perf_begin(_mag_sample_perf);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	mag_report.timestamp = hrt_absolute_time();
	mag_report.is_external = is_external();

	mag_report.x_raw = _last_raw_mag_x;
	mag_report.y_raw = _last_raw_mag_y;
	mag_report.z_raw = _last_raw_mag_z;

	float xraw_f = mag_report.x_raw;
	float yraw_f = mag_report.y_raw;
	float zraw_f = mag_report.z_raw;

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

	mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
	mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
	mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
	mag_report.scaling = _mag_range_scale;
	mag_report.range_ga = (float)_mag_range_ga;
	mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);

	mag_report.temperature = _last_temperature;
	mag_report.device_id = _mag->_device_id.devid;

	_mag_reports->force(&mag_report);

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

	if (!(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
	}

	_mag_read++;

	/* stop the perf counter */
	perf_end(_mag_sample_perf);
}

int
PMW3901::collect()
{
	int ret = OK;
	int16_t delta_x_raw, delta_y_raw;
	float delta_x, delta_y;

	perf_begin(_sample_perf);

	uint64_t timestamp = hrt_absolute_time();
	uint64_t dt_flow = timestamp - _previous_collect_timestamp;
	_previous_collect_timestamp = timestamp;

	_flow_dt_sum_usec += dt_flow;

	readMotionCount(delta_x_raw, delta_y_raw);

	_flow_sum_x += delta_x_raw;
	_flow_sum_y += delta_y_raw;

	// returns if the collect time has not been reached
	if (_flow_dt_sum_usec < _collect_time) {

		return ret;
	}

	delta_x = (float)_flow_sum_x / 500.0f;		// proportional factor + convert from pixels to radians
	delta_y = (float)_flow_sum_y / 500.0f;		// proportional factor + convert from pixels to radians

	struct optical_flow_s report;

	report.timestamp = timestamp;

	report.pixel_flow_x_integral = static_cast<float>(delta_x);
	report.pixel_flow_y_integral = static_cast<float>(delta_y);

	// rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
	float zeroval = 0.0f;
	rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
	rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);

	report.frame_count_since_last_readout = 4;				//microseconds
	report.integration_timespan = _flow_dt_sum_usec; 		//microseconds

	report.sensor_id = 0;

	// This sensor doesn't provide any quality metric. However if the sensor is unable to calculate the optical flow it will
	// output 0 for the delta. Hence, we set the measurement to "invalid" (quality = 0) if the values are smaller than FLT_EPSILON
	if (fabsf(report.pixel_flow_x_integral) < FLT_EPSILON && fabsf(report.pixel_flow_y_integral) < FLT_EPSILON) {
		report.quality = 0;

	} else {
		report.quality = 255;
	}

	/* No gyro on this board */
	report.gyro_x_rate_integral = NAN;
	report.gyro_y_rate_integral = NAN;
	report.gyro_z_rate_integral = NAN;

	// set (conservative) specs according to datasheet
	report.max_flow_rate = 5.0f;       // Datasheet: 7.4 rad/s
	report.min_ground_distance = 0.1f; // Datasheet: 80mm
	report.max_ground_distance = 30.0f; // Datasheet: infinity

	_flow_dt_sum_usec = 0;
	_flow_sum_x = 0;
	_flow_sum_y = 0;

	if (_optical_flow_pub == nullptr) {

		_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &report);

	} else {

		orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &report);
	}

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

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

	perf_end(_sample_perf);

	return ret;

}

int
IST8310::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t     x[2];
		uint8_t     y[2];
		uint8_t     z[2];
	} report_buffer;
#pragma pack(pop)
	struct {
		int16_t     x, y, z;
	} report;

	int ret;
	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	const bool sensor_is_external = external();

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.is_external = sensor_is_external;
	new_report.error_count = perf_event_count(_comms_errors);
	new_report.scaling = _range_scale;
	new_report.device_id = _device_id.devid;

	/*
	 * @note  We could read the status register here, which could tell us that
	 *        we were too early and that the output registers are still being
	 *        written.  In the common case that would just slow us down, and
	 *        we're better off just never being early.
	 */

	/* get measurements from the device */
	ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("I2C read error");
		goto out;
	}

	/* swap the data we just received */
	report.x = (((int16_t)report_buffer.x[1]) << 8) | (int16_t)report_buffer.x[0];
	report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
	report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];


	/*
	 * Check if value makes sense according to the FSR and Resolution of
	 * this sensor, discarding outliers
	 */
	if (report.x > IST8310_MAX_VAL_XY || report.x < IST8310_MIN_VAL_XY ||
	    report.y > IST8310_MAX_VAL_XY || report.y < IST8310_MIN_VAL_XY ||
	    report.z > IST8310_MAX_VAL_Z  || report.z < IST8310_MIN_VAL_Z) {
		perf_count(_range_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* temperature measurement is not available on IST8310 */
	new_report.temperature = 0;

	/*
	 * raw outputs
	 *
	 * Sensor doesn't follow right hand rule, swap x and y to make it obey
	 * it.
	 */
	new_report.x_raw = report.y;
	new_report.y_raw = report.x;
	new_report.z_raw = report.z;

	/* scale values for output */
	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;
	new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	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_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH);

//.........这里部分代码省略.........

int
PX4FLOW::collect()
{
    int ret = -EIO;

    /* read from the sensor */
    uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };

    perf_begin(_sample_perf);

    if (PX4FLOW_REG == 0x00) {
        ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
    }

    if (PX4FLOW_REG == 0x16) {
        ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
    }

    if (ret < 0) {
        DEVICE_DEBUG("error reading from sensor: %d", ret);
        perf_count(_comms_errors);
        perf_end(_sample_perf);
        return ret;
    }

    if (PX4FLOW_REG == 0) {
        memcpy(&f, val, I2C_FRAME_SIZE);
        memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
    }

    if (PX4FLOW_REG == 0x16) {
        memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
    }


    struct optical_flow_s report;

    report.timestamp = hrt_absolute_time();
    report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
    report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
    report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
    report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
    report.quality = f_integral.qual; //0:bad ; 255 max quality
    report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
    report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
    report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
    report.integration_timespan = f_integral.integration_timespan; //microseconds
    report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
    report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius

    report.sensor_id = 0;

    /* rotate measurements according to parameter */
    float zeroval = 0.0f;
    rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);

    if (_px4flow_topic == nullptr) {
        _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);

    } else {
        /* publish it */
        orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
    }

    /* publish to the distance_sensor topic as well */
    struct distance_sensor_s distance_report;
    distance_report.timestamp = report.timestamp;
    distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
    distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
    distance_report.current_distance = report.ground_distance_m;
    distance_report.covariance = 0.0f;
    distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
    /* TODO: the ID needs to be properly set */
    distance_report.id = 0;
    distance_report.orientation = 8;

    orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);

    /* post a report to the ring */
    if (_reports->force(&report)) {
        perf_count(_buffer_overflows);
    }

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

    ret = OK;

    perf_end(_sample_perf);
    return ret;
}

int
PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
{
    switch (cmd) {

    case SENSORIOCSPOLLRATE: {
        switch (arg) {

        /* switching to manual polling */
        case SENSOR_POLLRATE_MANUAL:
            stop();
            _measure_ticks = 0;
            return OK;

        /* external signalling (DRDY) not supported */
        case SENSOR_POLLRATE_EXTERNAL:

        /* zero would be bad */
        case 0:
            return -EINVAL;

        /* set default/max polling rate */
        case SENSOR_POLLRATE_MAX:
        case SENSOR_POLLRATE_DEFAULT: {
            /* do we need to start internal polling? */
            bool want_start = (_measure_ticks == 0);

            /* set interval for next measurement to minimum legal value */
            _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);

            /* if we need to start the poll state machine, do it */
            if (want_start) {
                start();
            }

            return OK;
        }

        /* adjust to a legal polling interval in Hz */
        default: {
            /* do we need to start internal polling? */
            bool want_start = (_measure_ticks == 0);

            /* convert hz to tick interval via microseconds */
            unsigned ticks = USEC2TICK(1000000 / arg);

            /* check against maximum rate */
            if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
                return -EINVAL;
            }

            /* update interval for next measurement */
            _measure_ticks = ticks;

            /* if we need to start the poll state machine, do it */
            if (want_start) {
                start();
            }

            return OK;
        }
        }
    }

    case SENSORIOCGPOLLRATE:
        if (_measure_ticks == 0) {
            return SENSOR_POLLRATE_MANUAL;
        }

        return (1000 / _measure_ticks);

    case SENSORIOCSQUEUEDEPTH: {
        /* lower bound is mandatory, upper bound is a sanity check */
        if ((arg < 1) || (arg > 100)) {
            return -EINVAL;
        }

        irqstate_t flags = irqsave();

        if (!_reports->resize(arg)) {
            irqrestore(flags);
            return -ENOMEM;
        }

        irqrestore(flags);

        return OK;
    }

    case SENSORIOCGQUEUEDEPTH:
        return _reports->size();

    case SENSORIOCSROTATION:
        _sensor_rotation = (enum Rotation)arg;
        return OK;

    case SENSORIOCGROTATION:
        return _sensor_rotation;

    case SENSORIOCRESET:
//.........这里部分代码省略.........