void AttitudePositionEstimatorEKF::pollData()
{
	//Update arming status
	bool armedUpdate;
	orb_check(_armedSub, &armedUpdate);

	if (armedUpdate) {
		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
	}

	//Update Gyro and Accelerometer
	static Vector3f lastAngRate;
	static Vector3f lastAccel;
	bool accel_updated = false;

	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

	static hrt_abstime last_accel = 0;
	static hrt_abstime last_mag = 0;

	if (last_accel != _sensor_combined.accelerometer_timestamp) {
		accel_updated = true;

	} else {
		accel_updated = false;
	}

	last_accel = _sensor_combined.accelerometer_timestamp;


	// Copy gyro and accel
	_last_sensor_timestamp = _sensor_combined.timestamp;
	IMUmsec = _sensor_combined.timestamp / 1e3;
	IMUusec = _sensor_combined.timestamp;

	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;

	/* guard against too large deltaT's */
	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
		deltaT = 0.01f;
	}

	_last_run = _sensor_combined.timestamp;

	// Always store data, independent of init status
	/* fill in last data set */
	_ekf->dtIMU = deltaT;

	int last_gyro_main = _gyro_main;

	if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {

		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
		_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
		_gyro_main = 0;
		_gyro_valid = true;

	} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {

		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
		_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
		_gyro_main = 1;
		_gyro_valid = true;

	} else {
		_gyro_valid = false;
	}

	if (last_gyro_main != _gyro_main) {
		mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
	}

	if (!_gyro_valid) {
		/* keep last value if he hit an out of band value */
		lastAngRate = _ekf->angRate;

	} else {
		perf_count(_perf_gyro);
	}

	if (accel_updated) {

		int last_accel_main = _accel_main;

		/* fail over to the 2nd accel if we know the first is down */
		if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
			_accel_main = 0;

		} else {
			_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
//.........这里部分代码省略.........

void BlockLocalPositionEstimator::update()
{

	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		/* poll error, count it in perf */
		perf_count(_err_perf);
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
		detectDistanceSensors();
	}

	// reset pos, vel, and terrain on arming
	if (!_lastArmedState && armedState) {

		// we just armed, we are at home position on the ground
		_x(X_x) = 0;
		_x(X_y) = 0;

		// the pressure altitude of home may have drifted, so we don't
		// reset z to zero

		// reset flow integral
		_flowX = 0;
		_flowY = 0;

		// we aren't moving, all velocities are zero
		_x(X_vx) = 0;
		_x(X_vy) = 0;
		_x(X_vz) = 0;

		// assume we are on the ground, so terrain alt is local alt
		_x(X_tz) = _x(X_z);

		// reset lowpass filter as well
		_xLowPass.setState(_x);
	}

	_lastArmedState = armedState;

	// see which updates are available
	bool flowUpdated = _sub_flow.updated();
	bool paramsUpdated = _sub_param_update.updated();
	bool baroUpdated = _sub_sensor.updated();
	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
	bool homeUpdated = _sub_home.updated();
	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
	bool mocapUpdated = _sub_mocap.updated();
	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// update home position projection
	if (homeUpdated) {
		updateHome();
	}

	// is xy valid?
	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();

	if (_validXY) {
		// if valid and gps has timed out, set to not valid
		if (!xy_stddev_ok && !_gpsInitialized) {
			_validXY = false;
		}

	} else {
		if (xy_stddev_ok) {
			_validXY = true;
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	if (_validZ) {
		// if valid and baro has timed out, set to not valid
		if (!z_stddev_ok && !_baroInitialized) {
//.........这里部分代码省略.........

float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(PX4_ISFINITE(ctl_data.roll) &&
	      PX4_ISFINITE(ctl_data.pitch) &&
	      PX4_ISFINITE(ctl_data.pitch_rate) &&
	      PX4_ISFINITE(ctl_data.yaw_rate) &&
	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
	      PX4_ISFINITE(ctl_data.airspeed_min) &&
	      PX4_ISFINITE(ctl_data.airspeed_max) &&
	      PX4_ISFINITE(ctl_data.scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;

	/* apply turning offset to desired bodyrate setpoint*/
	/* flying inverted (wings upside down)*/
	bool inverted = false;
	float constrained_roll;
	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));

	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
		inverted = true;
		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (ctl_data.roll > 0.0f) {
			/* right hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));

		} else {
			/* left hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* input conditioning */
	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);

	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
	   Availible on google books 8/11/2015: 
	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
				  		tanf(constrained_roll) * sinf(constrained_roll)));

	if (inverted) {
		body_fixed_turn_offset = -body_fixed_turn_offset;
	}

	/* Finally add the turn offset to your bodyrate setpoint*/
	_bodyrate_setpoint += body_fixed_turn_offset;


	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	//Calculate D term
	float diff_error = -ctl_data.pitch_rate;
	float d_term = (_k_d * _k_f) * (diff_error - _rate_diff_error_state);
//.........这里部分代码省略.........

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;

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

	report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	report.x = _gyro_filter_x.apply(report.x);
	report.y = _gyro_filter_y.apply(report.y);
	report.z = _gyro_filter_z.apply(report.z);

	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;

	report.scaling = _gyro_range_scale;
	report.range_rad_s = _gyro_range_rad_s;
//.........这里部分代码省略.........

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) >> 6;

	switch (status) {
	case 0:
		break;

	case 1:
		/* fallthrough */
	case 2:
		/* fallthrough */
	case 3:
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return -EAGAIN;
	}

	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.0f * dT_raw) / 2047) - 50;

	// Calculate differential pressure. As its centered around 8000
	// and can go positive or negative
	const float P_min = -1.0f;
	const float P_max = 1.0f;
	const float PSI_to_Pa = 6894.757f;
	/*
	  this equation is an inversion of the equation in the
	  pressure transfer function figure on page 4 of the datasheet

	  We negate the result so that positive differential pressures
	  are generated when the bottom port is used as the static
	  port on the pitot and top port is used as the dynamic port
	 */
	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;

        // correct for 5V rail voltage if possible
        voltage_correction(diff_press_pa_raw, temperature);

	float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
	
	/*
	  note that we return both the absolute value with offset
	  applied and a raw value without the offset applied. This
	  makes it possible for higher level code to detect if the
	  user has the tubes connected backwards, and also makes it
	  possible to correctly use offsets calculated by a higher
	  level airspeed driver.

	  With the above calculation the MS4525 sensor will produce a
	  positive number when the top port is used as a dynamic port
	  and bottom port is used as the static port

	  Also note that the _diff_pres_offset is applied before the
	  fabsf() not afterwards. It needs to be done this way to
	  prevent a bias at low speeds, but this also means that when
	  setting a offset you must set it based on the raw value, not
	  the offset value
	 */
	
	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.differential_pressure_raw_pa = diff_press_pa_raw;
	report.voltage = 0;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub > 0 && !(_pub_blocked)) {
		/* publish it */
//.........这里部分代码省略.........

void *
fat_dma_alloc(size_t size)
{
	perf_count(g_dma_perf);
	return gran_alloc(dma_allocator, size);
}

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

void
LSM303D::measure()
{
	// if the accel doesn't have any data ready then re-schedule
	// for 100 microseconds later. This ensures we don't double
	// read a value and then miss the next value
	if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
		perf_count(_accel_reschedules);
		hrt_call_delay(&_accel_call, 100);
		return;
	}
	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
		perf_count(_reg1_resets);
		reset();
		return;
	}

	/* 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_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 = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));

	/*
	 * 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();
        accel_report.error_count = 0; // not reported

	accel_report.x_raw = raw_accel_report.x;
	accel_report.y_raw = raw_accel_report.y;
	accel_report.z_raw = raw_accel_report.z;

	float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	accel_report.x = _accel_filter_x.apply(x_in_new);
	accel_report.y = _accel_filter_y.apply(y_in_new);
	accel_report.z = _accel_filter_z.apply(z_in_new);

	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 */
	poll_notify(POLLIN);

	if (_accel_topic > 0 && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
	}

	_accel_read++;

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

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

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

void AttitudePositionEstimatorEKF::task_main()
{
	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	_ekf = new AttPosEKF();

	_filter_start_time = hrt_absolute_time();

	if (!_ekf) {
		warnx("OUT OF MEM!");
		return;
	}

	/*
	 * do subscriptions
	 */
	_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_home_sub = orb_subscribe(ORB_ID(home_position));
	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_armedSub = orb_subscribe(ORB_ID(actuator_armed));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vstatus_sub, 200);

	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_sensor_combined_sub, 9);

	/* sets also parameters in the EKF object */
	parameters_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;

	fds[1].fd = _sensor_combined_sub;
	fds[1].events = POLLIN;

	_gps.vel_n_m_s = 0.0f;
	_gps.vel_e_m_s = 0.0f;
	_gps.vel_d_m_s = 0.0f;

	_task_running = true;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("POLL ERR %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);
		perf_count(_loop_intvl);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run estimator if gyro updated */
		if (fds[1].revents & POLLIN) {

			/* check vehicle status for changes to publication state */
			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
			vehicle_status_poll();

			perf_count(_perf_gyro);

			/* Reset baro reference if switching to HIL, reset sensor states */
			if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
				/* system is in HIL now, wait for measurements to come in one last round */
				usleep(60000);

				/* now read all sensor publications to ensure all real sensor data is purged */
				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

				/* set sensors to de-initialized state */
				_gyro_valid = false;
//.........这里部分代码省略.........

int AttitudePositionEstimatorEKF::check_filter_state()
{
	/*
	 *    CHECK IF THE INPUT DATA IS SANE
	 */

	struct ekf_status_report ekf_report;

	int check = _ekf->CheckAndBound(&ekf_report);

	const char *const feedback[] = { 0,
					 "NaN in states, resetting",
					 "stale sensor data, resetting",
					 "got initial position lock",
					 "excessive gyro offsets",
					 "velocity diverted, check accel config",
					 "excessive covariances",
					 "unknown condition, resetting"
				       };

	// Print out error condition
	if (check) {
		unsigned warn_index = static_cast<unsigned>(check);
		unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));

		if (max_warn_index < warn_index) {
			warn_index = max_warn_index;
		}

		// Do not warn about accel offset if we have no position updates
		if (!(warn_index == 5 && _ekf->staticMode)) {
			warnx("reset: %s", feedback[warn_index]);
			mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
		}
	}

	struct estimator_status_s rep;

	memset(&rep, 0, sizeof(rep));

	// If error flag is set, we got a filter reset
	if (check && ekf_report.error) {

		// Count the reset condition
		perf_count(_perf_reset);

	} else if (_ekf_logging) {
		_ekf->GetFilterState(&ekf_report);
	}

	if (_ekf_logging || check) {
		rep.timestamp = hrt_absolute_time();

		rep.nan_flags |= (((uint8_t)ekf_report.angNaN)		<< 0);
		rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN)	<< 1);
		rep.nan_flags |= (((uint8_t)ekf_report.KHNaN)		<< 2);
		rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN)		<< 3);
		rep.nan_flags |= (((uint8_t)ekf_report.PNaN)		<< 4);
		rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN)	<< 5);
		rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN)	<< 6);
		rep.nan_flags |= (((uint8_t)ekf_report.statesNaN)	<< 7);

		rep.health_flags |= (((uint8_t)ekf_report.velHealth)	<< 0);
		rep.health_flags |= (((uint8_t)ekf_report.posHealth)	<< 1);
		rep.health_flags |= (((uint8_t)ekf_report.hgtHealth)	<< 2);
		rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive)	<< 3);
		rep.health_flags |= (((uint8_t)ekf_report.onGround)	<< 4);
		rep.health_flags |= (((uint8_t)ekf_report.staticMode)	<< 5);
		rep.health_flags |= (((uint8_t)ekf_report.useCompass)	<< 6);
		rep.health_flags |= (((uint8_t)ekf_report.useAirspeed)	<< 7);

		rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout)	<< 0);
		rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout)	<< 1);
		rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout)	<< 2);
		rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout)	<< 3);

		if (_debug > 10) {

			if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
				warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
				      ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
				      ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
				      ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
				      ((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
			}

			if (rep.timeout_flags) {
				warnx("timeout: %s%s%s%s",
				      ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
				      ((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
				      ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
				      ((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
			}
		}

		// Copy all states or at least all that we can fit
		size_t ekf_n_states = ekf_report.n_states;
		size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
		rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;

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

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();

#if L3GD20_USE_DRDY
	// if the gyro doesn't have any data ready then re-schedule
	// for 100 microseconds later. This ensures we don't double
	// read a value and then miss the next value
	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
		perf_count(_reschedules);
		hrt_call_delay(&_call, 100);
                perf_end(_sample_perf);
		return;
	}
#endif

	/* 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 L3GD20_USE_DRDY
        if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
            /*
              we waited for DRDY, but did not see DRDY on all axes
              when we captured. That means a transfer error of some sort
             */
            perf_count(_errors);
            return;
        }
#endif
	/*
	 * 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;

	report.temperature_raw = raw_report.temp;

	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
//.........这里部分代码省略.........

/**
   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) {
//.........这里部分代码省略.........

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

			/* Simple handling of failsafe: deploy parachute if failsafe is on */
			if (_vcontrol_mode.flag_control_termination_enabled) {
				_actuators_airframe.control[7] = 1.0f;
				//warnx("_actuators_airframe.control[1] = 1.0f;");
			} else {
				_actuators_airframe.control[7] = 0.0f;
				//warnx("_actuators_airframe.control[1] = -1.0f;");
			}

			/* if we are in rotary wing mode, do nothing */
			if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
				continue;
			}

			/* default flaps to center */
			float flaps_control = 0.0f;

			/* map flaps by default to manual if valid */
			if (isfinite(_manual.flaps)) {
				flaps_control = _manual.flaps;
			}

			/* decide if in stabilized or full manual control */
			if (_vcontrol_mode.flag_control_attitude_enabled) {
				/* scale around tuning airspeed */
				float airspeed;

				/* if airspeed is not updating, we assume the normal average speed */
				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
					airspeed = _parameters.airspeed_trim;
					if (nonfinite) {
						perf_count(_nonfinite_input_perf);
					}
				} else {
					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
				}

				/*
				 * For scaling our actuators using anything less than the min (close to stall)
				 * speed doesn't make any sense - its the strongest reasonable deflection we
				 * want to do in flight and its the baseline a human pilot would choose.
				 *
				 * Forcing the scaling to this value allows reasonable handheld tests.
				 */

				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);

				float roll_sp = _parameters.rollsp_offset_rad;
				float pitch_sp = _parameters.pitchsp_offset_rad;
				float yaw_manual = 0.0f;
				float throttle_sp = 0.0f;

				/* Read attitude setpoint from uorb if
				 * - velocity control or position control is enabled (pos controller is running)
				 * - manual control is disabled (another app may send the setpoint, but it should
				 *   for sure not be set from the remote control values)
				 */
				if (_vcontrol_mode.flag_control_auto_enabled ||
						!_vcontrol_mode.flag_control_manual_enabled) {
					/* read in attitude setpoint from attitude setpoint uorb topic */
					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

void
PX4IO_serial_f7::_do_interrupt()
{
	uint32_t sr = rISR;	/* get UART status register */

	if (sr & USART_ISR_RXNE) {
		(void)rRDR;	/* read DR to clear RXNE */
	}

	rICR = sr & rISR_ERR_FLAGS_MASK;	/* clear flags */

	if (sr & (USART_ISR_ORE |		/* overrun error - packet was too big for DMA or DMA was too slow */
		  USART_ISR_NF |		/* noise error - we have lost a byte due to noise */
		  USART_ISR_FE)) {		/* framing error - start/stop bit lost or line break */

		/*
		 * If we are in the process of listening for