C++ LowPassFilter2p::apply方法代码示例

//.........这里部分代码省略.........
				/* velocity updated */
				if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
					float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
					/* calculate acceleration in body frame */
					_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
				}

				_vel_prev_t = _gpos.timestamp;
				_vel_prev = vel;
			}

		} else {
			/* position data is outdated, reset acceleration */
			_pos_acc.zero();
			_vel_prev.zero();
			_vel_prev_t = 0;
		}

		// Time from previous iteration
		hrt_abstime now = hrt_absolute_time();
		float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;
		last_time = now;

		if (dt > _dt_max) {
			dt = _dt_max;
		}

		if (!update(dt)) {
			continue;
		}

		Vector<3> euler = _q.to_euler();

		struct vehicle_attitude_s att = {};
		att.timestamp = sensors.timestamp;

		att.roll = euler(0);
		att.pitch = euler(1);
		att.yaw = euler(2);

		/* the complimentary filter should reflect the true system
		 * state, but we need smoother outputs for the control system
		 */
		att.rollspeed = _lp_roll_rate.apply(_rates(0));
		att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
		att.yawspeed = _lp_yaw_rate.apply(_rates(2));

		for (int i = 0; i < 3; i++) {
			att.g_comp[i] = _accel(i) - _pos_acc(i);
		}

		/* copy offsets */
		memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));

		Matrix<3, 3> R = _q.to_dcm();

		/* copy rotation matrix */
		memcpy(&att.R[0], R.data, sizeof(att.R));
		att.R_valid = true;

		att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
		att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
		att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());

		if (_att_pub == nullptr) {
			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

		} else {
			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
		}

		struct control_state_s ctrl_state = {};

		ctrl_state.timestamp = sensors.timestamp;

		/* Attitude quaternions for control state */
		ctrl_state.q[0] = _q(0);

		ctrl_state.q[1] = _q(1);

		ctrl_state.q[2] = _q(2);

		ctrl_state.q[3] = _q(3);

		/* Attitude rates for control state */
		ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));

		ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));

		ctrl_state.yaw_rate = _rates(2);

		/* Publish to control state topic */
		if (_ctrl_state_pub == nullptr) {
			_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);

		} else {
			orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
		}
	}
}

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

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

    float diff_press_pa = fabsf(diff_press_pa_raw);

    /*
      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_filtered_pa =  _filter.apply(diff_press_pa);

    /* the dynamics of the filter can make it overshoot into the negative range */
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			ev_data.angErr = _default_ev_ang_noise;

			// use timestamp from external computer, clocks are synchronized when using MAVROS
			_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
		}

		orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);

		if (vehicle_land_detected_updated) {
			orb_copy(ORB_ID(vehicle_land_detected), vehicle_land_detected_sub, &vehicle_land_detected);
			_ekf.set_in_air_status(!vehicle_land_detected.landed);
		}

		// run the EKF update and output
		if (_ekf.update()) {

			matrix::Quaternion<float> q;
			_ekf.copy_quaternion(q.data());

			float velocity[3];
			_ekf.get_velocity(velocity);

			float gyro_rad[3];

			{
				// generate control state data
				control_state_s ctrl_state = {};
				float gyro_bias[3] = {};
				_ekf.get_gyro_bias(gyro_bias);
				ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time();
				gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
				gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
				gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
				ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
				ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
				ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
				ctrl_state.roll_rate_bias = gyro_bias[0];
				ctrl_state.pitch_rate_bias = gyro_bias[1];
				ctrl_state.yaw_rate_bias = gyro_bias[2];

				// Velocity in body frame
				Vector3f v_n(velocity);
				matrix::Dcm<float> R_to_body(q.inversed());
				Vector3f v_b = R_to_body * v_n;
				ctrl_state.x_vel = v_b(0);
				ctrl_state.y_vel = v_b(1);
				ctrl_state.z_vel = v_b(2);


				// Local Position NED
				float position[3];
				_ekf.get_position(position);
				ctrl_state.x_pos = position[0];
				ctrl_state.y_pos = position[1];
				ctrl_state.z_pos = position[2];

				// Attitude quaternion
				q.copyTo(ctrl_state.q);

				_ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter);

				// Acceleration data
				matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);

				float accel_bias[3];
				_ekf.get_accel_bias(accel_bias);

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

	float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

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

	math::Vector<3> gval(xin, yin, zin);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
	report.x_integral = gval_integrated(0);
	report.y_integral = gval_integrated(1);
	report.z_integral = gval_integrated(2);

	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;

	report.scaling = _gyro_range_scale;
	report.range_rad_s = _gyro_range_rad_s;

	_reports->force(&report);

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

		/* publish for subscribers */
		if (!(_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
		}
	}

	_read++;

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

void
L3GD20::measure()
{
#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);
		return;
	}
#endif

	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_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);

	/* 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 ((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 = 0; // not recorded
	
	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.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;

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

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

void
LSM303D::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_accel_report;
#pragma pack(pop)

	accel_report accel_report;

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

	/* fetch data from the sensor */
	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.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);

	/* publish for subscribers */
	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);

	_accel_read++;

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

//.........这里部分代码省略.........
		// Position of local NED origin in GPS / WGS84 frame
		struct map_projection_reference_s ekf_origin = {};
		_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
		lpos.xy_global =
			_ekf->position_is_valid();          // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
		lpos.z_global = true;                                // true if z is valid and has valid global reference (ref_alt)
		lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
		lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees

		// The rotation of the tangent plane vs. geographical north
		lpos.yaw = 0.0f;

		lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters
		lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate
		lpos.surface_bottom_timestamp	= 0; // Time when new bottom surface found
		lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid

		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
		// TODO: Should use sqrt of filter position variances
		lpos.eph = gps.eph;
		lpos.epv = gps.epv;

		// publish vehicle local position data
		if (_lpos_pub == nullptr) {
			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);

		} else {
			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
		}

		// generate control state data
		control_state_s ctrl_state = {};
		ctrl_state.timestamp = hrt_absolute_time();
		ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
		ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
		ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);

		ctrl_state.q[0] = q(0);
		ctrl_state.q[1] = q(1);
		ctrl_state.q[2] = q(2);
		ctrl_state.q[3] = q(3);

		// publish control state data
		if (_control_state_pub == nullptr) {
			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);

		} else {
			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
		}

		// generate vehicle attitude data
		att.q[0] = q(0);
		att.q[1] = q(1);
		att.q[2] = q(2);
		att.q[3] = q(3);
		att.q_valid = true;

		att.rollspeed = sensors.gyro_rad_s[0];
		att.pitchspeed = sensors.gyro_rad_s[1];
		att.yawspeed = sensors.gyro_rad_s[2];

		// publish vehicle attitude data
		if (_att_pub == nullptr) {
			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

		} else {

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

	/* read from the sensor */
#pragma pack(push, 1)
	struct {
		float		temperature;
		float		diff_pressure;
	} airspeed_report;
#pragma pack(pop)


	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));

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

	uint8_t status = 0;

	switch (status) {
	case 0:
		break;

	case 1:

	/* fallthrough */
	case 2:

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

	float temperature = airspeed_report.temperature;
	float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar

	struct differential_pressure_s report;

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

	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.temperature = temperature;
	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);

	report.differential_pressure_raw_pa = diff_press_pa_raw;
	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;
}

//.........这里部分代码省略.........
			flow.quality = optical_flow.quality;
			flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
			flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
			flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
			flow.dt = optical_flow.integration_timespan;

			if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
			    PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
				_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
			}
		}

		if (range_finder_updated) {
			_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
		}

		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);

		if (vehicle_land_detected_updated) {
			orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
			_ekf.set_in_air_status(!vehicle_land_detected.landed);
		}

		// run the EKF update and output
		if (_ekf.update()) {
			// generate vehicle attitude quaternion data
			struct vehicle_attitude_s att = {};
			_ekf.copy_quaternion(att.q);
			matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);

			// generate control state data
			control_state_s ctrl_state = {};
			ctrl_state.timestamp = hrt_absolute_time();
			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);

			// Velocity in body frame
			float velocity[3];
			_ekf.get_velocity(velocity);
			Vector3f v_n(velocity);
			matrix::Dcm<float> R_to_body(q.inversed());
			Vector3f v_b = R_to_body * v_n;
			ctrl_state.x_vel = v_b(0);
			ctrl_state.y_vel = v_b(1);
			ctrl_state.z_vel = v_b(2);


			// Local Position NED
			float position[3];
			_ekf.get_position(position);
			ctrl_state.x_pos = position[0];
			ctrl_state.y_pos = position[1];
			ctrl_state.z_pos = position[2];

			// Attitude quaternion
			ctrl_state.q[0] = q(0);
			ctrl_state.q[1] = q(1);
			ctrl_state.q[2] = q(2);
			ctrl_state.q[3] = q(3);

			// Acceleration data
			matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};

			float accel_bias[3];
			_ekf.get_accel_bias(accel_bias);

int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
{
	/* Check if calibration values are still up-to-date. */
	bool updated;
	orb_check(_param_update_sub, &updated);

	if (updated) {
		parameter_update_s parameter_update;
		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);

		_update_accel_calibration();
		_update_gyro_calibration();
		_update_mag_calibration();
	}

	accel_report accel_report = {};
	gyro_report gyro_report = {};
	mag_report mag_report = {};

	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();

	// ACCEL

	float xraw_f = data.accel_m_s2_x;
	float yraw_f = data.accel_m_s2_y;
	float zraw_f = data.accel_m_s2_z;

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

	// MPU9250 driver from DriverFramework does not provide any raw values
	// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
	accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
	accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
	accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];

	// adjust values according to the calibration
	float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
	float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
	float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.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);

	matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
	matrix::Vector3f aval_integrated;

	_accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
	accel_report.x_integral = aval_integrated(0);
	accel_report.y_integral = aval_integrated(1);
	accel_report.z_integral = aval_integrated(2);

	// GYRO

	xraw_f = data.gyro_rad_s_x;
	yraw_f = data.gyro_rad_s_y;
	zraw_f = data.gyro_rad_s_z;

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

	// MPU9250 driver from DriverFramework does not provide any raw values
	// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
	gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
	gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
	gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];

	// adjust values according to the calibration
	float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
	float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
	float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;

	gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new);
	gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);
	gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new);

	matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	matrix::Vector3f gval_integrated;

	_gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
	gyro_report.x_integral = gval_integrated(0);
	gyro_report.y_integral = gval_integrated(1);
	gyro_report.z_integral = gval_integrated(2);

	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
	// and wait for more packets.
	if (!data.is_last_fifo_sample) {
		return 0;
	}

	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
	// Therefore, only publish every forth time.
	++_publish_count;

	if (_publish_count < 4) {
		return 0;
	}

	_publish_count = 0;
//.........这里部分代码省略.........

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

		/* time from previous iteration */
		hrt_abstime now = hrt_absolute_time();
		float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;
		last_time = now;

		if (dt > _dt_max) {
			dt = _dt_max;
		}

		if (!update(dt)) {
			continue;
		}

		Vector<3> euler = _q.to_euler();

		struct vehicle_attitude_s att = {};
		att.timestamp = sensors.timestamp;

		att.roll = euler(0);
		att.pitch = euler(1);
		att.yaw = euler(2);

		att.rollspeed = _rates(0);
		att.pitchspeed = _rates(1);
		att.yawspeed = _rates(2);

		for (int i = 0; i < 3; i++) {
			att.g_comp[i] = _accel(i) - _pos_acc(i);
		}

		/* copy offsets */
		memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));

		Matrix<3, 3> R = _q.to_dcm();

		/* copy rotation matrix */
		memcpy(&att.R[0], R.data, sizeof(att.R));
		att.R_valid = true;
		memcpy(&att.q[0], _q.data, sizeof(att.q));
		att.q_valid = true;

		att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
		att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
		att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());

		/* the instance count is not used here */
		int att_inst;
		orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);

		{
			struct control_state_s ctrl_state = {};

			ctrl_state.timestamp = sensors.timestamp;

			/* attitude quaternions for control state */
			ctrl_state.q[0] = _q(0);
			ctrl_state.q[1] = _q(1);
			ctrl_state.q[2] = _q(2);
			ctrl_state.q[3] = _q(3);

			/* attitude rates for control state */
			ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));

			ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));

			ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));

			/* Airspeed - take airspeed measurement directly here as no wind is estimated */
			if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
			    && _airspeed.timestamp > 0) {
				ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
				ctrl_state.airspeed_valid = true;

			} else {
				ctrl_state.airspeed_valid = false;
			}

			/* the instance count is not used here */
			int ctrl_inst;
			/* publish to control state topic */
			orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
		}

		{
			struct estimator_status_s est = {};

			est.timestamp = sensors.timestamp;
			est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
			est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
			est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);

			/* the instance count is not used here */
			int est_inst;
			/* publish to control state topic */
			orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
		}
	}
}

//.........这里部分代码省略.........
	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;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	/*
	  we have logs where the accelerometers get stuck at a fixed
	  large value. We want to detect this and mark the sensor as
	  being faulty
	 */
	if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
	    fabsf(_last_accel[1] - y_in_new) < 0.001f &&
	    fabsf(_last_accel[2] - z_in_new) < 0.001f &&
	    fabsf(x_in_new) > 20 &&
	    fabsf(y_in_new) > 20 &&
	    fabsf(z_in_new) > 20) {
		_constant_accel_count += 1;

	} else {
		_constant_accel_count = 0;
	}

	if (_constant_accel_count > 100) {
		// we've had 100 constant accel readings with large
		// values. The sensor is almost certainly dead. We
		// will raise the error_count so that the top level
		// flight code will know to avoid this sensor, but
		// we'll still give the data so that it can be logged
		// and viewed
		perf_count(_bad_values);
		_constant_accel_count = 0;
	}

	_last_accel[0] = x_in_new;
	_last_accel[1] = y_in_new;
	_last_accel[2] = z_in_new;

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

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
	accel_report.x_integral = aval_integrated(0);
	accel_report.y_integral = aval_integrated(1);
	accel_report.z_integral = aval_integrated(2);

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

	/* return device ID */
	accel_report.device_id = _device_id.devid;

	_accel_reports->force(&accel_report);

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

		if (!(_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);
}

void AttitudeEstimatorQ::task_main()
{

#ifdef __PX4_POSIX
	perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
	perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
	perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif

	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));

	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));

	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));

	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

	update_parameters(true);

	hrt_abstime last_time = 0;

	px4_pollfd_struct_t fds[1] = {};
	fds[0].fd = _sensors_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {
		int ret = px4_poll(fds, 1, 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			PX4_WARN("Q POLL ERROR");
			continue;

		} else if (ret == 0) {
			// Poll timeout, do nothing
			PX4_WARN("Q POLL TIMEOUT");
			continue;
		}

		update_parameters(false);

		// Update sensors
		sensor_combined_s sensors;

		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
			// Feed validator with recent sensor data

			if (sensors.timestamp > 0) {
				// Filter gyro signal since it is not fildered in the drivers.
				_gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
				_gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
				_gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
			}

			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
				// Filter accel signal since it is not fildered in the drivers.
				_accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
				_accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
				_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);

				if (_accel.length() < 0.01f) {
					PX4_DEBUG("WARNING: degenerate accel!");
					continue;
				}
			}

			if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
				_mag(0) = sensors.magnetometer_ga[0];
				_mag(1) = sensors.magnetometer_ga[1];
				_mag(2) = sensors.magnetometer_ga[2];

				if (_mag.length() < 0.01f) {
					PX4_DEBUG("WARNING: degenerate mag!");
					continue;
				}
			}

			_data_good = true;
		}

		// Update vision and motion capture heading
		bool vision_updated = false;
		orb_check(_vision_sub, &vision_updated);

		bool mocap_updated = false;
		orb_check(_mocap_sub, &mocap_updated);

		if (vision_updated) {
			orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
			math::Quaternion q(_vision.q);

			math::Matrix<3, 3> Rvis = q.to_dcm();
			math::Vector<3> v(1.0f, 0.0f, 0.4f);

			// Rvis is Rwr (robot respect to world) while v is respect to world.
			// Hence Rvis must be transposed having (Rwr)' * Vw
			// Rrw * Vw = vn. This way we have consistency
//.........这里部分代码省略.........

void
L3GD20::measure()
{
	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_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);

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

	/*
	 * 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 = 0; // not recorded
	
	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.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;

	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.scaling = _gyro_range_scale;
	report.range_rad_s = _gyro_range_rad_s;

	_reports->force(&report);

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

	/* publish for subscribers */
	if (_gyro_topic > 0)
		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);

	_read++;

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

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