int
MPU9250::self_test()
{
	if (perf_event_count(_sample_perf) == 0) {
		measure();
	}

	/* return 0 on success, 1 else */
	return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}

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

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]);

	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();

	if (diff_pres_pa_raw < FLT_EPSILON) {
		// a zero value indicates no measurement
		// since the noise floor has been arbitrarily killed
		// it defeats our stuck sensor detection - the best we
		// can do is to output some numerical noise to show
		// that we are still correctly sampling.
		diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
	}

	// The raw value still should be compensated for the known offset
	diff_pres_pa_raw -= _diff_pres_offset;

	report.error_count = perf_event_count(_comms_errors);

	// XXX we may want to smooth out the readings to remove noise.
	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
	report.differential_pressure_raw_pa = diff_pres_pa_raw;
	report.temperature = -1000.0f;
	report.device_id = _device_id.devid;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

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

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}

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

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	// read the high and low byte distance registers
	uint8_t distance_reg = LL40LS_DISTHIGH_REG;
	ret = transfer(&distance_reg, 1, &val[0], sizeof(val));

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

	uint16_t distance = (val[0] << 8) | val[1];
	float si_units = distance * 0.01f; /* cm to m */
	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
		report.valid = 1;
	}
	else {
		report.valid = 0;
	}

	/* publish it, if we are the primary */
	if (_range_finder_topic >= 0) {
		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
	}

	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
TRONE::collect()
{
	int ret = -EIO;

	/* read from the sensor */
	uint8_t val[3] = {0, 0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 3);

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

	uint16_t distance = (val[0] << 8) | val[1];
	float si_units = distance *  0.001f; /* mm to m */
	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	report.minimum_distance = get_minimum_distance();
	report.maximum_distance = get_maximum_distance();
	report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;


	/* publish it, if we are the primary */
	if (_range_finder_topic >= 0) {
		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
	}

	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 LidarLiteI2C::measure()
{
	int ret;

	if (_pause_measurements) {
		// we are in print_registers() and need to avoid
		// acquisition to keep the I2C peripheral on the
		// sensor active
		return OK;
	}

	/*
	 * Send the command to begin a measurement.
	 */
	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
	ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);

	if (OK != ret) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("i2c::transfer returned %d", ret);

		// if we are getting lots of I2C transfer errors try
		// resetting the sensor
		if (perf_event_count(_comms_errors) % 10 == 0) {
			perf_count(_sensor_resets);
			reset_sensor();
		}

		return ret;
	}

	// remember when we sent the acquire so we can know when the
	// acquisition has timed out
	_acquire_time_usec = hrt_absolute_time();
	ret = OK;

	return ret;
}

void
L3GD20::measure()
{
	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		int8_t		temp;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_report;
#pragma pack(pop)

	gyro_report report;

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

	check_registers();

	/* fetch data from the sensor */
	memset(&raw_report, 0, sizeof(raw_report));
	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));

	if (!(raw_report.status & STATUS_ZYXDA)) {
		perf_end(_sample_perf);
		perf_count(_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.
	 */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_bad_registers);

	switch (_orientation) {

	case SENSOR_BOARD_ROTATION_000_DEG:
		/* keep axes in place */
		report.x_raw = raw_report.x;
		report.y_raw = raw_report.y;
		break;

	case SENSOR_BOARD_ROTATION_090_DEG:
		/* swap x and y */
		report.x_raw = raw_report.y;
		report.y_raw = raw_report.x;
		break;

	case SENSOR_BOARD_ROTATION_180_DEG:
		/* swap x and y and negate both */
		report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
		report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
		break;

	case SENSOR_BOARD_ROTATION_270_DEG:
		/* swap x and y and negate y */
		report.x_raw = raw_report.y;
		report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
		break;
	}

	report.z_raw = raw_report.z;

#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
	int16_t tx = -report.y_raw;
	int16_t ty = -report.x_raw;
	int16_t tz = -report.z_raw;
	report.x_raw = tx;
	report.y_raw = ty;
	report.z_raw = tz;
#endif




	report.temperature_raw = raw_report.temp;

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

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

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

//.........这里部分代码省略.........
	/* we did increment the index to the next position already, so just add the additional fields */
	_linebuf_index += (ret - 1);

	_last_read = hrt_absolute_time();

	if (_linebuf_index < 1) {
		/* we need at least the two end bytes to make sense of this string */
		return -EAGAIN;

	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {

		if (_linebuf_index >= readlen - 1) {
			/* we have a full buffer, but no line ending - abort */
			_linebuf_index = 0;
			perf_count(_comms_errors);
			return -ENOMEM;
		} else {
			/* incomplete read, reschedule ourselves */
			return -EAGAIN;
		}
	}

	char *end;
	float si_units;
	bool valid;

	/* enforce line ending */
	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);

	_linebuf[lend] = '\0';

	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
		si_units = -1.0f;
		valid = false;

	} else {

		/* we need to find a dot in the string, as we're missing the meters part else */
		valid = false;

		/* wipe out partially read content from last cycle(s), check for dot */
		for (int i = 0; i < (lend - 2); i++) {
			if (_linebuf[i] == '\n') {
				char buf[sizeof(_linebuf)];
				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
			}

			if (_linebuf[i] == '.') {
				valid = true;
			}
		}

		if (valid) {
			si_units = strtod(_linebuf, &end);

			/* we require at least 3 characters for a valid number */
			if (end > _linebuf + 3) {
				valid = true;
			} else {
				si_units = -1.0f;
				valid = false;
			}
		}
	}

	debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));

	/* done with this chunk, resetting - even if invalid */
	_linebuf_index = 0;

	/* if its invalid, there is no reason to forward the value */
	if (!valid) {
		perf_count(_comms_errors);
		return -EINVAL;
	}

	struct range_finder_report report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.distance = si_units;
	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

	/* publish it */
	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

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

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}

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

#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_gyro_report;
#pragma pack(pop)

	struct gyro_report gyro_report;

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

	check_registers();

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

	/* fetch data from the sensor */
	memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
	raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
	transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));

	if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		return;
	}

	/*
	 * The TEMP register contains an 8-bit 2's complement temperature value with a range
	 * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only
	 * compensated (factory trim values applied) when the device is operating in the Active
	 * mode and actively measuring the angular rate.
	 */

	if ((_read % _current_rate) == 0) {
		_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
		gyro_report.temperature = _last_temperature;
	}

	/*
	 * 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.
	 */

	gyro_report.timestamp = hrt_absolute_time();

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

	gyro_report.x_raw = swap16(raw_gyro_report.x);
	gyro_report.y_raw = swap16(raw_gyro_report.y);
	gyro_report.z_raw = swap16(raw_gyro_report.z);

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

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

	float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	gyro_report.x = _gyro_filter_x.apply(x_in_new);
	gyro_report.y = _gyro_filter_y.apply(y_in_new);
	gyro_report.z = _gyro_filter_z.apply(z_in_new);

	matrix::Vector3f gval(x_in_new, y_in_new, z_in_new);
	matrix::Vector3f gval_integrated;

	bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
	gyro_report.x_integral = gval_integrated(0);
//.........这里部分代码省略.........

void
MPU9250_mag::measure(struct ak8963_regs data)
{
	bool mag_notify = true;

	if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
		perf_count(_mag_duplicates);
		return;
	}

	/* monitor for if data overrun flag is ever set */
	if (data.st1 & 0x02) {
		perf_count(_mag_overruns);
	}

	/* monitor for if magnetic sensor overflow flag is ever set noting that st2
	 * is usually not even refreshed, but will always be in the same place in the
	 * mpu's buffers regardless, hence the actual count would be bogus
	 */
	if (data.st2 & 0x08) {
		perf_count(_mag_overflows);
	}

	mag_report	mrb;
	mrb.timestamp = hrt_absolute_time();

	/*
	 * Align axes - note the accel & gryo are also re-aligned so this
	 *              doesn't look obvious with the datasheet
	 */
	mrb.x_raw =  data.x;
	mrb.y_raw = -data.y;
	mrb.z_raw = -data.z;

	float xraw_f =  data.x;
	float yraw_f = -data.y;
	float zraw_f = -data.z;

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

	mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale;
	mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale;
	mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale;
	mrb.range_ga = (float)48.0;
	mrb.scaling = _mag_range_scale;
	mrb.temperature = _parent->_last_temperature;

	mrb.error_count = perf_event_count(_mag_errors);

	_mag_reports->force(&mrb);

	/* notify anyone waiting for data */
	if (mag_notify) {
		poll_notify(POLLIN);
	}

	if (mag_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb);
	}
}

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

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];

	if (diff_pres_pa_raw == 0) {
		// a zero value means the pressure sensor cannot give us a
		// value. We need to return, and not report a value or the
		// caller could end up using this value as part of an
		// average
		perf_count(_comms_errors);
		DEVICE_LOG("zero value from sensor");
		return -1;
	}

	// The raw value still should be compensated for the known offset
	diff_pres_pa_raw -= _diff_pres_offset;

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_pres_pa_raw;
	}

	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);

	// XXX we may want to smooth out the readings to remove noise.
	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
	report.differential_pressure_raw_pa = diff_pres_pa_raw;
	report.temperature = -1000.0f;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

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

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}

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

    /* read from the sensor */
    uint8_t val[2] = {0, 0};

    perf_begin(_sample_perf);

    // read the high and low byte distance registers
    uint8_t distance_reg = LL40LS_DISTHIGH_REG;
    ret = transfer(&distance_reg, 1, &val[0], sizeof(val));

    if (ret < 0) {
        if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
            /*
              NACKs from the sensor are expected when we
              read before it is ready, so only consider it
              an error if more than 100ms has elapsed.
             */
            debug("error reading from sensor: %d", ret);
            perf_count(_comms_errors);
            if (perf_event_count(_comms_errors) % 10 == 0) {
                perf_count(_sensor_resets);
                reset_sensor();
            }
        }
        perf_end(_sample_perf);
        // if we are getting lots of I2C transfer errors try
        // resetting the sensor
        return ret;
    }

    uint16_t distance = (val[0] << 8) | val[1];
    float si_units = distance * 0.01f; /* cm to m */
    struct range_finder_report report;

    if (distance == 0) {
        _zero_counter++;
        if (_zero_counter == 20) {
            /* we have had 20 zeros in a row - reset the
               sensor. This is a known bad state of the
               sensor where it returns 16 bits of zero for
               the distance with a trailing NACK, and
               keeps doing that even when the target comes
               into a valid range.
            */
            _zero_counter = 0;
            perf_end(_sample_perf);
            perf_count(_sensor_zero_resets);
            return reset_sensor();
        }
    } else {
        _zero_counter = 0;
    }

    _last_distance = distance;

    /* this should be fairly close to the end of the measurement, so the best approximation of the time */
    report.timestamp = hrt_absolute_time();
    report.error_count = perf_event_count(_comms_errors);
    report.distance = si_units;
    report.minimum_distance = get_minimum_distance();
    report.maximum_distance = get_maximum_distance();
    if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
        report.valid = 1;
    }
    else {
        report.valid = 0;
    }

    /* publish it, if we are the primary */
    if (_range_finder_topic >= 0) {
        orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
    }

    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
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 = -EIO;
	uint8_t	cmd;


	perf_begin(_sample_perf);
	struct mag_report new_report;

	/* 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 */
	cmd = ADDR_DATA_OUT_X_MSB;
	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		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;
	}

	/*
	 * 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 */

	/*
	 * 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.
	 */

#ifdef PX4_I2C_BUS_ONBOARD
	if (_bus == PX4_I2C_BUS_ONBOARD) {
		/* to align the sensor axes with the board, x and y need to be flipped */
		new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
		/* flip axes and negate value for y */
		new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
		/* z remains z */
		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
	} else {
#endif
		/* 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 */
		new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
		/* flip axes and negate value for y */
		new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
//.........这里部分代码省略.........

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

	/* read from the sensor */
	uint8_t val[4] = {0, 0, 0, 0};


	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 4);

	if (ret < 0) {
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;
	}

	uint8_t status = val[0] & 0xC0;

	if (status == 2) {
		log("err: stale data");
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;
	} else if (status == 3) {
		log("err: fault");
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;                
	}

	int16_t dp_raw = 0, dT_raw = 0;
	dp_raw = (val[0] << 8) + val[1];
	/* mask the used bits */
	dp_raw = 0x3FFF & dp_raw;
	dT_raw = (val[2] << 8) + val[3];
	dT_raw = (0xFFE0 & dT_raw) >> 5;
	float temperature = ((200 * dT_raw) / 2047) - 50;

	/* calculate differential pressure. As its centered around 8000
	 * and can go positive or negative, enforce absolute value
	*/
	const float P_min = -1.0f;
	const float P_max = 1.0f;
	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
        if (diff_press_pa < 0.0f)
            diff_press_pa = 0.0f;

	struct differential_pressure_s report;

	/* track maximum differential pressure measured (so we can work out top speed). */
	if (diff_press_pa > _max_differential_pressure_pa) {
	    _max_differential_pressure_pa = diff_press_pa;
	}

	report.timestamp = hrt_absolute_time();
        report.error_count = perf_event_count(_comms_errors);
	report.temperature = temperature;
	report.differential_pressure_pa = diff_press_pa;
	report.voltage = 0;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	/* announce the airspeed if needed, just publish else */
	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);

	new_report(report);

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

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}

int
LPS25H::collect()
{
#pragma pack(push, 1)
	struct {
		uint8_t		status;
		uint8_t		p_xl, p_l, p_h;
		int16_t		t;
	} report;
#pragma pack(pop)

	int	ret;

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

	/* 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 1 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 : MSB enables register address auto-increment */
	ret = _interface->read(ADDR_STATUS_REG | (1 << 7), (uint8_t *)&report, sizeof(report));

	if (ret != OK) {
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return ret;
	}

	/* get measurements from the device */
	new_report.temperature = 42.5 + (report.t / 480);

	/* raw pressure */
	uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);

	/* Pressure and MSL in mBar */
	double p = raw / 4096.0;
	double msl = _msl_pressure / 100.0;

	double alt = (1.0 - pow(p / msl, 0.190263)) * 44330.8;

	new_report.pressure = p;
	new_report.altitude = alt;

	/* get device ID */
	new_report.device_id = _device_id.devid;

	if (!(_pub_blocked)) {

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

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

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

	_last_report = new_report;

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

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


	ret = OK;

	perf_end(_sample_perf);
	return ret;
}

/**
   check for extreme accelerometer values and log to a file on the SD card
 */
void
LSM303D::check_extremes(const accel_report *arb)
{
	const float extreme_threshold = 30;
        static bool boot_ok = false;
	bool is_extreme = (fabsf(arb->x) > extreme_threshold && 
			   fabsf(arb->y) > extreme_threshold && 
			   fabsf(arb->z) > extreme_threshold);
	if (is_extreme) {
		perf_count(_extreme_values);
		// force accel logging on if we see extreme values
		_accel_logging_enabled = true;
	} else {
            boot_ok = true;
        }

	if (! _accel_logging_enabled) {
		// logging has been disabled by user, close
		if (_accel_log_fd != -1) {
			::close(_accel_log_fd);
			_accel_log_fd = -1;
		}
		return;
	}
	if (_accel_log_fd == -1) {
		// keep last 10 logs
		::unlink(ACCEL_LOGFILE ".9");
		for (uint8_t i=8; i>0; i--) {
			uint8_t len = strlen(ACCEL_LOGFILE)+3;
			char log1[len], log2[len];
			snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
			snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
			::rename(log1, log2);
		}
		::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");

		// open the new logfile
		_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
		if (_accel_log_fd == -1) {
			return;
		}
	}

	uint64_t now = hrt_absolute_time();
	// log accels at 1Hz
	if (_last_log_us == 0 ||
	    now - _last_log_us > 1000*1000) {
		_last_log_us = now;
		::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
			  (unsigned long long)arb->timestamp, 
			  (double)arb->x, (double)arb->y, (double)arb->z,
			  (int)arb->x_raw,
			  (int)arb->y_raw,
			  (int)arb->z_raw,
			  (unsigned)boot_ok);
	}

        const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, 
                                    ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, 
                                    ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, 
                                    ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, 
                                    ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, 
                                    ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, 
                                    ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, 
                                    ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, 
                                    ADDR_ACT_THS, ADDR_ACT_DUR,
                                    ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, 
                                    ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
        uint8_t regval[sizeof(reglist)];
        for (uint8_t i=0; i<sizeof(reglist); i++) {
            regval[i] = read_reg(reglist[i]);
        }

	// log registers at 10Hz when we have extreme values, or 0.5 Hz without
	if (_last_log_reg_us == 0 ||
	    (is_extreme && (now - _last_log_reg_us > 250*1000)) ||
	    (now - _last_log_reg_us > 10*1000*1000)) {
		_last_log_reg_us = now;
		::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
		for (uint8_t i=0; i<sizeof(reglist); i++) {
			::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
		}
		::dprintf(_accel_log_fd, "\n");
	}

	// fsync at 0.1Hz
	if (now - _last_log_sync_us > 10*1000*1000) {
		_last_log_sync_us = now;
		::fsync(_accel_log_fd);
	}

	// play alarm every 10s if we have had an extreme value
	if (perf_event_count(_extreme_values) != 0 && 
	    (now - _last_log_alarm_us > 10*1000*1000)) {
		_last_log_alarm_us = now;
		int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
		if (tfd != -1) {
//.........这里部分代码省略.........

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

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

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

	uint16_t distance = val[0] << 8 | val[1];
	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
	struct range_finder_report report;

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

	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
	if (addr_ind.size() == 1) {
		report.distance = si_units;

		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
			report.distance_vector[i] = 0;
		}

		report.just_updated = 0;

	} else {
		/* for multiple sonars connected */

		/* don't use the orginial single sonar variable */
		report.distance = 0;

		/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values 			of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest 			value for each connected sonar */
		_latest_sonar_measurements[_cycle_counter] = si_units;

		for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
			report.distance_vector[i] = _latest_sonar_measurements[i];
		}

		/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been 		collected as this could be of use for Kalman filters */
		report.just_updated = _index_counter;

		/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
			report.distance_vector[addr_ind.size() + i] = 0;
		}
	}

	report.minimum_distance = get_minimum_distance();
	report.maximum_distance = get_maximum_distance();
	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;

	/* publish it, if we are the primary */
	if (_range_finder_topic >= 0) {
		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
	}

	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

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

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}

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
SF0X::collect()
{
    int	ret;

    perf_begin(_sample_perf);

    /* clear buffer if last read was too long ago */
    uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

    /* the buffer for read chars is buflen minus null termination */
    char readbuf[sizeof(_linebuf)];
    unsigned readlen = sizeof(readbuf) - 1;

    /* read from the sensor (uart buffer) */
    ret = ::read(_fd, &readbuf[0], readlen);

    if (ret < 0) {
        debug("read err: %d", ret);
        perf_count(_comms_errors);
        perf_end(_sample_perf);

        /* only throw an error if we time out */
        if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
            return ret;

        } else {
            return -EAGAIN;
        }
    } else if (ret == 0) {
        return -EAGAIN;
    }

    _last_read = hrt_absolute_time();

    float si_units;
    bool valid = false;

    for (int i = 0; i < ret; i++) {
        if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
            valid = true;
        }
    }

    if (!valid) {
        return -EAGAIN;
    }

    debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));

    struct range_finder_report report;

    /* this should be fairly close to the end of the measurement, so the best approximation of the time */
    report.timestamp = hrt_absolute_time();
    report.error_count = perf_event_count(_comms_errors);
    report.distance = si_units;
    report.minimum_distance = get_minimum_distance();
    report.maximum_distance = get_maximum_distance();
    report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

    /* publish it */
    orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);

    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
BAROSIM::collect()
{
	int ret;

#pragma pack(push, 1)
	struct {
		float		pressure;
		float		altitude;
		float		t