void AttitudeEstimatorQ::task_main()
{
	_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));

	_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 (_mavlink_fd < 0) {
			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
		}

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout, do nothing
			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

			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {

				/* ignore empty fields */
				if (sensors.gyro_timestamp[i] > 0) {

					float gyro[3];

					for (unsigned j = 0; j < 3; j++) {
						if (sensors.gyro_integral_dt[i] > 0) {
							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);

						} else {
							/* fall back to angular rate */
							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
						}
					}

					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
				}

				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
						 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
					       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
			}

			int best_gyro, best_accel, best_mag;

			// Get best measurement values
			hrt_abstime curr_time = hrt_absolute_time();
			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
			_mag.set(_voter_mag.get_best(curr_time, &best_mag));

			if (_accel.length() < 0.01f || _mag.length() < 0.01f) {
				warnx("WARNING: degenerate accel / mag!");
				continue;
			}

			_data_good = true;

			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
					   _voter_accel.failover_count() > 0 ||
					   _voter_mag.failover_count() > 0)) {

				_failsafe = true;
				mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
			}

			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
						    _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
						    _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {

				if (_vibration_warning_timestamp == 0) {
					_vibration_warning_timestamp = curr_time;

				} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
					_vibration_warning = true;
					mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
									 (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
									 (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
									 (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
				}

			} else {
				_vibration_warning_timestamp = 0;
			}
		}

		// 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
			_vision_hdg = Rvis.transposed() * v;
		}

		if (mocap_updated) {
			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
			math::Quaternion q(_mocap.q);
			math::Matrix<3, 3> Rmoc = q.to_dcm();

			math::Vector<3> v(1.0f, 0.0f, 0.4f);

			// Rmoc is Rwr (robot respect to world) while v is respect to world.
			// Hence Rmoc must be transposed having (Rwr)' * Vw
			// Rrw * Vw = vn. This way we have consistency
			_mocap_hdg = Rmoc.transposed() * v;
		}

		// Check for timeouts on data
		if (_ext_hdg_mode == 1) {
			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);

		} else if (_ext_hdg_mode == 2) {
			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
		}

		bool gpos_updated;
		orb_check(_global_pos_sub, &gpos_updated);

		if (gpos_updated) {
			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);

			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
				/* set magnetic declination automatically */
				_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
			}
		}

		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
			/* position data is actual */
			if (gpos_updated) {
				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);

				/* 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);
		}
	}
}
示例#2
0
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 */
    if (report.differential_pressure_filtered_pa < 0.0f) {
        report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
    }

    report.differential_pressure_raw_pa = diff_press_pa_raw;
    report.max_differential_pressure_pa = _max_differential_pressure_pa;

    if (_airspeed_pub > 0 && !(_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;
}
示例#3
0
void Ekf2::task_main()
{
	// subscribe to relevant topics
	int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	int params_sub = orb_subscribe(ORB_ID(parameter_update));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	int status_sub = orb_subscribe(ORB_ID(vehicle_status));

	px4_pollfd_struct_t fds[2] = {};
	fds[0].fd = sensors_sub;
	fds[0].events = POLLIN;
	fds[1].fd = params_sub;
	fds[1].events = POLLIN;

	// initialise parameter cache
	updateParams();

	// initialize data structures outside of loop
	// because they will else not always be
	// properly populated
	sensor_combined_s sensors = {};
	vehicle_gps_position_s gps = {};
	airspeed_s airspeed = {};
	optical_flow_s optical_flow = {};
	distance_sensor_s range_finder = {};
	vehicle_land_detected_s vehicle_land_detected = {};
	vehicle_local_position_s ev_pos = {};
	vehicle_attitude_s ev_att = {};
	vehicle_status_s vehicle_status = {};

	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}

		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), params_sub, &update);
			updateParams();

			// fetch sensor data in next loop
			continue;

		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}

		bool gps_updated = false;
		bool airspeed_updated = false;
		bool optical_flow_updated = false;
		bool range_finder_updated = false;
		bool vehicle_land_detected_updated = false;
		bool vision_position_updated = false;
		bool vision_attitude_updated = false;
		bool vehicle_status_updated = false;

		orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
		// update all other topics if they have new data

		orb_check(status_sub, &vehicle_status_updated);

		if (vehicle_status_updated) {
			orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
		}

		orb_check(gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
		}

		orb_check(airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
		}

		orb_check(optical_flow_sub, &optical_flow_updated);

		if (optical_flow_updated) {
			orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
		}

		orb_check(range_finder_sub, &range_finder_updated);

		if (range_finder_updated) {
			orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder);

			if (range_finder.min_distance > range_finder.current_distance
			    || range_finder.max_distance < range_finder.current_distance) {
				range_finder_updated = false;
			}
		}

		orb_check(ev_pos_sub, &vision_position_updated);

		if (vision_position_updated) {
			orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
		}

		orb_check(ev_att_sub, &vision_attitude_updated);

		if (vision_attitude_updated) {
			orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
		}

		// in replay mode we are getting the actual timestamp from the sensor topic
		hrt_abstime now = 0;

		if (_replay_mode) {
			now = sensors.timestamp;

		} else {
			now = hrt_absolute_time();
		}

		// push imu data into estimator
		float gyro_integral[3];
		gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
		gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
		gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
		float accel_integral[3];
		accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
		accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
		accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
		_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
				gyro_integral, accel_integral);

		// read mag data
		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_mag_us = 0;

		} else {
			if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) {
				_timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative;

				// If the time last used by the EKF is less than specified, then accumulate the
				// data and push the average when the 50msec is reached.
				_mag_time_sum_ms += _timestamp_mag_us / 1000;
				_mag_sample_count++;
				_mag_data_sum[0] += sensors.magnetometer_ga[0];
				_mag_data_sum[1] += sensors.magnetometer_ga[1];
				_mag_data_sum[2] += sensors.magnetometer_ga[2];
				uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;

				if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) {
					float mag_sample_count_inv = 1.0f / (float)_mag_sample_count;
					float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv, _mag_data_sum[1] *mag_sample_count_inv, _mag_data_sum[2] *mag_sample_count_inv};
					_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
					_mag_time_ms_last_used = mag_time_ms;
					_mag_time_sum_ms = 0;
					_mag_sample_count = 0;
					_mag_data_sum[0] = 0.0f;
					_mag_data_sum[1] = 0.0f;
					_mag_data_sum[2] = 0.0f;

				}
			}
		}

		// read baro data
		if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_balt_us = 0;

		} else {
			if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
				_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;

				// If the time last used by the EKF is less than specified, then accumulate the
				// data and push the average when the 50msec is reached.
				_balt_time_sum_ms += _timestamp_balt_us / 1000;
				_balt_sample_count++;
				_balt_data_sum += sensors.baro_alt_meter;
				uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;

				if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
					_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
					_balt_time_ms_last_used = balt_time_ms;
					_balt_time_sum_ms = 0;
					_balt_sample_count = 0;
					_balt_data_sum = 0.0f;

				}
			}
		}

		// read gps data if available
		if (gps_updated) {
			struct gps_message gps_msg = {};
			gps_msg.time_usec = gps.timestamp;
			gps_msg.lat = gps.lat;
			gps_msg.lon = gps.lon;
			gps_msg.alt = gps.alt;
			gps_msg.fix_type = gps.fix_type;
			gps_msg.eph = gps.eph;
			gps_msg.epv = gps.epv;
			gps_msg.sacc = gps.s_variance_m_s;
			gps_msg.vel_m_s = gps.vel_m_s;
			gps_msg.vel_ned[0] = gps.vel_n_m_s;
			gps_msg.vel_ned[1] = gps.vel_e_m_s;
			gps_msg.vel_ned[2] = gps.vel_d_m_s;
			gps_msg.vel_ned_valid = gps.vel_ned_valid;
			gps_msg.nsats = gps.satellites_used;
			//TODO add gdop to gps topic
			gps_msg.gdop = 0.0f;

			_ekf.setGpsData(gps.timestamp, &gps_msg);

		}

		// only set airspeed data if condition for airspeed fusion are met
		bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing
				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f;

		if (fuse_airspeed) {
			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
			_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
		}

		// only fuse synthetic sideslip measurements if conditions are met
		bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get();
		_ekf.set_fuse_beta_flag(fuse_beta);

		if (optical_flow_updated) {
			flow_message flow;
			flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
			flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
			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);
		}

		// get external vision data
		// if error estimates are unavailable, use parameter defined defaults
		if (vision_position_updated || vision_attitude_updated) {
			ext_vision_message ev_data;
			ev_data.posNED(0) = ev_pos.x;
			ev_data.posNED(1) = ev_pos.y;
			ev_data.posNED(2) = ev_pos.z;
			Quaternion q(ev_att.q);
			ev_data.quat = q;

			// position measurement error from parameters. TODO : use covariances from topic
			ev_data.posErr = _default_ev_pos_noise;
			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);
				ctrl_state.x_acc = acceleration(0) - accel_bias[0];
				ctrl_state.y_acc = acceleration(1) - accel_bias[1];
				ctrl_state.z_acc = acceleration(2) - accel_bias[2];

				// compute lowpass filtered horizontal acceleration
				acceleration = R_to_body.transpose() * acceleration;
				_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
						acceleration(1) * acceleration(1));
				ctrl_state.horz_acc_mag = _acc_hor_filt;

				ctrl_state.airspeed_valid = false;

				// use estimated velocity for airspeed estimate
				if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
					// use measured airspeed
					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 if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
					if (_ekf.local_position_is_valid()) {
						ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
						ctrl_state.airspeed_valid = true;
					}

				} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
					// do nothing, airspeed has been declared as non-valid above, controllers
					// will handle this assuming always trim airspeed
				}

				// 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 quaternion data
				struct vehicle_attitude_s att = {};
				att.timestamp = _replay_mode ? now : hrt_absolute_time();

				q.copyTo(att.q);

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

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

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

			// generate vehicle local position data
			struct vehicle_local_position_s lpos = {};
			float pos[3] = {};

			lpos.timestamp = _replay_mode ? now : hrt_absolute_time();

			// Position of body origin in local NED frame
			_ekf.get_position(pos);
			lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f;
			lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f;
			lpos.z = pos[2];

			// Velocity of body origin in local NED frame (m/s)
			lpos.vx = velocity[0];
			lpos.vy = velocity[1];
			lpos.vz = velocity[2];

			// TODO: better status reporting
			lpos.xy_valid = _ekf.local_position_is_valid();
			lpos.z_valid = true;
			lpos.v_xy_valid = _ekf.local_position_is_valid();
			lpos.v_z_valid = true;

			// Position of local NED origin in GPS / WGS84 frame
			struct map_projection_reference_s ekf_origin = {};
			// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
			_ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
			lpos.xy_global = _ekf.global_position_is_valid();
			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
			matrix::Eulerf euler(q);
			lpos.yaw = euler.psi();

			float terrain_vpos;
			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
			lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
			lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
			lpos.surface_bottom_timestamp	= hrt_absolute_time(); // Time when new bottom surface found

			// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
			Vector3f pos_var, vel_var;
			_ekf.get_pos_var(pos_var);
			_ekf.get_vel_var(vel_var);
			lpos.eph = sqrtf(pos_var(0) + pos_var(1));
			lpos.epv = sqrtf(pos_var(2));

			// get state reset information of position and velocity
			_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
			_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
			_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
			_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);

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

			if (_ekf.global_position_is_valid()) {
				// generate and publish global position data
				struct vehicle_global_position_s global_pos = {};

				global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds

				double est_lat, est_lon, lat_pre_reset, lon_pre_reset;
				map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
				global_pos.lat = est_lat; // Latitude in degrees
				global_pos.lon = est_lon; // Longitude in degrees
				map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset,
							 &lon_pre_reset);
				global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset;
				global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset;
				global_pos.lat_lon_reset_counter = lpos.xy_reset_counter;

				global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
				_ekf.get_posD_reset(&global_pos.delta_alt, &global_pos.alt_reset_counter);
				// global altitude has opposite sign of local down position
				global_pos.delta_alt *= -1.0f;

				global_pos.vel_n = velocity[0]; // Ground north velocity, m/s
				global_pos.vel_e = velocity[1]; // Ground east velocity, m/s
				global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s

				global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI.

				global_pos.eph = sqrtf(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
				global_pos.epv = sqrtf(pos_var(2)); // Standard deviation of position vertically

				if (lpos.dist_bottom_valid) {
					global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
					global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid

				} else {
					global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
					global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
				}

				// TODO use innovatun consistency check timouts to set this
				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning

				global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)

				if (_vehicle_global_position_pub == nullptr) {
					_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

				} else {
					orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
				}
			}

		} else if (_replay_mode) {
			// in replay mode we have to tell the replay module not to wait for an update
			// we do this by publishing an attitude with zero timestamp
			struct vehicle_attitude_s att = {};
			att.timestamp = now;

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

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

		// publish estimator status
		struct estimator_status_s status = {};
		status.timestamp = _replay_mode ? now : hrt_absolute_time();
		_ekf.get_state_delayed(status.states);
		_ekf.get_covariances(status.covariances);
		_ekf.get_gps_check_status(&status.gps_check_fail_flags);
		_ekf.get_control_mode(&status.control_mode_flags);
		_ekf.get_filter_fault_status(&status.filter_fault_flags);
		_ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
						&status.vel_test_ratio, &status.pos_test_ratio,
						&status.hgt_test_ratio, &status.tas_test_ratio,
						&status.hagl_test_ratio);
		bool dead_reckoning;
		_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning);
		_ekf.get_ekf_soln_status(&status.solution_status_flags);
		_ekf.get_imu_vibe_metrics(status.vibe);

		if (_estimator_status_pub == nullptr) {
			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);

		} else {
			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
		}

		// Publish wind estimate
		struct wind_estimate_s wind_estimate = {};
		wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time();
		wind_estimate.windspeed_north = status.states[22];
		wind_estimate.windspeed_east = status.states[23];
		wind_estimate.covariance_north = status.covariances[22];
		wind_estimate.covariance_east = status.covariances[23];

		if (_wind_pub == nullptr) {
			_wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate);

		} else {
			orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate);
		}

		// publish estimator innovation data
		{
			struct ekf2_innovations_s innovations = {};
			innovations.timestamp = _replay_mode ? now : hrt_absolute_time();
			_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
			_ekf.get_mag_innov(&innovations.mag_innov[0]);
			_ekf.get_heading_innov(&innovations.heading_innov);
			_ekf.get_airspeed_innov(&innovations.airspeed_innov);
			_ekf.get_beta_innov(&innovations.beta_innov);
			_ekf.get_flow_innov(&innovations.flow_innov[0]);
			_ekf.get_hagl_innov(&innovations.hagl_innov);

			_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
			_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
			_ekf.get_heading_innov_var(&innovations.heading_innov_var);
			_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
			_ekf.get_beta_innov_var(&innovations.beta_innov_var);
			_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
			_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);

			_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);

			if (_estimator_innovations_pub == nullptr) {
				_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);

			} else {
				orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
			}

		}

		// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
		if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
			float decl_deg;
			_ekf.copy_mag_decl_deg(&decl_deg);
			_mag_declination_deg.set(decl_deg);
		}

		_prev_landed = vehicle_land_detected.landed;

		// publish ekf2_timestamps (using 0.1 ms relative timestamps)
		{
			ekf2_timestamps_s ekf2_timestamps;
			ekf2_timestamps.timestamp = sensors.timestamp;

			if (gps_updated) {
				// divide individually to get consistent rounding behavior
				ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (optical_flow_updated) {
				ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (range_finder_updated) {
				ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (airspeed_updated) {
				ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (vision_position_updated) {
				ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (vision_attitude_updated) {
				ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 -
						(int64_t)ekf2_timestamps.timestamp / 100);

			} else {
				ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
			}

			if (_ekf2_timestamps_pub == nullptr) {
				_ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps);

			} else {
				orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps);
			}
		}


		// publish replay message if in replay mode
		bool publish_replay_message = (bool)_param_record_replay_msg.get();

		if (publish_replay_message) {
			struct ekf2_replay_s replay = {};
			replay.timestamp = now;
			replay.gyro_integral_dt = sensors.gyro_integral_dt;
			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
			replay.magnetometer_timestamp = _timestamp_mag_us;
			replay.baro_timestamp = _timestamp_balt_us;
			memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
			memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
			replay.baro_alt_meter = sensors.baro_alt_meter;

			// only write gps data if we had a gps update.
			if (gps_updated) {
				replay.time_usec = gps.timestamp;
				replay.lat = gps.lat;
				replay.lon = gps.lon;
				replay.alt = gps.alt;
				replay.fix_type = gps.fix_type;
				replay.nsats = gps.satellites_used;
				replay.eph = gps.eph;
				replay.epv = gps.epv;
				replay.sacc = gps.s_variance_m_s;
				replay.vel_m_s = gps.vel_m_s;
				replay.vel_n_m_s = gps.vel_n_m_s;
				replay.vel_e_m_s = gps.vel_e_m_s;
				replay.vel_d_m_s = gps.vel_d_m_s;
				replay.vel_ned_valid = gps.vel_ned_valid;

			} else {
				// this will tell the logging app not to bother logging any gps replay data
				replay.time_usec = 0;
			}

			if (optical_flow_updated) {
				replay.flow_timestamp = optical_flow.timestamp;
				replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral;
				replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral;
				replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
				replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
				replay.flow_time_integral = optical_flow.integration_timespan;
				replay.flow_quality = optical_flow.quality;

			} else {
				replay.flow_timestamp = 0;
			}

			if (range_finder_updated) {
				replay.rng_timestamp = range_finder.timestamp;
				replay.range_to_ground = range_finder.current_distance;

			} else {
				replay.rng_timestamp = 0;
			}

			if (airspeed_updated) {
				replay.asp_timestamp = airspeed.timestamp;
				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;

			} else {
				replay.asp_timestamp = 0;
			}

			if (vision_position_updated || vision_attitude_updated) {
				replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
				replay.pos_ev[0] = ev_pos.x;
				replay.pos_ev[1] = ev_pos.y;
				replay.pos_ev[2] = ev_pos.z;
				replay.quat_ev[0] = ev_att.q[0];
				replay.quat_ev[1] = ev_att.q[1];
				replay.quat_ev[2] = ev_att.q[2];
				replay.quat_ev[3] = ev_att.q[3];
				// TODO : switch to covariances from topic later
				replay.pos_err = _default_ev_pos_noise;
				replay.ang_err = _default_ev_ang_noise;

			} else {
				replay.ev_timestamp = 0;
			}

			if (_replay_pub == nullptr) {
				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);

			} else {
				orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
			}
		}
	}

	orb_unsubscribe(sensors_sub);
	orb_unsubscribe(gps_sub);
	orb_unsubscribe(airspeed_sub);
	orb_unsubscribe(params_sub);
	orb_unsubscribe(optical_flow_sub);
	orb_unsubscribe(range_finder_sub);
	orb_unsubscribe(ev_pos_sub);
	orb_unsubscribe(vehicle_land_detected_sub);
	orb_unsubscribe(status_sub);

	delete ekf2::instance;
	ekf2::instance = nullptr;
}
示例#4
0
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);

	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);
}
示例#5
0
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);

	// apply user specified rotation
	rotate_3f(_rotation, report.x, report.y, 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 (!(_pub_blocked)) {
		/* publish it */
		orb_publish(_orb_id, _gyro_topic, &report);
	}

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
示例#6
0
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);
}
示例#7
0
void Ekf2::task_main()
{
	// subscribe to relevant topics
	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));

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

	// initialise parameter cache
	updateParams();

	vehicle_gps_position_s gps = {};

	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}

		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
			updateParams();

			// fetch sensor data in next loop
			continue;

		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}

		bool gps_updated = false;
		bool airspeed_updated = false;
		bool control_mode_updated = false;
		bool vehicle_status_updated = false;

		sensor_combined_s sensors = {};
		airspeed_s airspeed = {};
		vehicle_control_mode_s vehicle_control_mode = {};

		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);

		// update all other topics if they have new data
		orb_check(_gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
		}

		orb_check(_airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
		}

		// Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
		// TODO implement a global vehicle on-ground/in-air check
		orb_check(_control_mode_sub, &control_mode_updated);

		if (control_mode_updated) {
			orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode);
			_ekf->set_arm_status(vehicle_control_mode.flag_armed);
		}

		hrt_abstime now = hrt_absolute_time();
		// push imu data into estimator
		_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
				 &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);

		// read mag data
		_ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);

		// read baro data
		_ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);

		// read gps data if available
		if (gps_updated) {
			struct gps_message gps_msg = {};
			gps_msg.time_usec = gps.timestamp_position;
			gps_msg.lat = gps.lat;
			gps_msg.lon = gps.lon;
			gps_msg.alt = gps.alt;
			gps_msg.fix_type = gps.fix_type;
			gps_msg.eph = gps.eph;
			gps_msg.epv = gps.epv;
			gps_msg.sacc = gps.s_variance_m_s;
			gps_msg.time_usec_vel = gps.timestamp_velocity;
			gps_msg.vel_m_s = gps.vel_m_s;
			gps_msg.vel_ned[0] = gps.vel_n_m_s;
			gps_msg.vel_ned[1] = gps.vel_e_m_s;
			gps_msg.vel_ned[2] = gps.vel_d_m_s;
			gps_msg.vel_ned_valid = gps.vel_ned_valid;
			gps_msg.nsats = gps.satellites_used;
			//TODO add gdop to gps topic
			gps_msg.gdop = 0.0f;

			_ekf->setGpsData(gps.timestamp_position, &gps_msg);
		}

		// read airspeed data if available
		if (airspeed_updated) {
			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
		}

		// read vehicle status if available for 'landed' information
		orb_check(_vehicle_status_sub, &vehicle_status_updated);

		if (vehicle_status_updated) {
			struct vehicle_status_s status = {};
			orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
			_ekf->set_in_air_status(!status.condition_landed);
		}

		// run the EKF update
		_ekf->update();

		// generate vehicle attitude data
		struct vehicle_attitude_s att = {};
		att.timestamp = hrt_absolute_time();

		_ekf->copy_quaternion(att.q);
		matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
		matrix::Euler<float> euler(q);
		att.roll = euler(0);
		att.pitch = euler(1);
		att.yaw = euler(2);

		// generate vehicle local position data
		struct vehicle_local_position_s lpos = {};
		float pos[3] = {};
		float vel[3] = {};

		lpos.timestamp = hrt_absolute_time();

		// Position in local NED frame
		_ekf->copy_position(pos);
		lpos.x = pos[0];
		lpos.y = pos[1];
		lpos.z = pos[2];

		// Velocity in NED frame (m/s)
		_ekf->copy_velocity(vel);
		lpos.vx = vel[0];
		lpos.vy = vel[1];
		lpos.vz = vel[2];

		// TODO: better status reporting
		lpos.xy_valid = _ekf->position_is_valid();
		lpos.z_valid = true;
		lpos.v_xy_valid = _ekf->position_is_valid();
		lpos.v_z_valid = true;

		// 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 {
			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
		}

		// generate and publish global position data
		struct vehicle_global_position_s global_pos = {};

		if (_ekf->position_is_valid()) {
			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator

			global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
			global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds

			double est_lat, est_lon;
			map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
			global_pos.lat = est_lat; // Latitude in degrees
			global_pos.lon = est_lon; // Longitude in degrees

			global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters

			global_pos.vel_n = vel[0]; // Ground north velocity, m/s
			global_pos.vel_e = vel[1]; // Ground east velocity, m/s
			global_pos.vel_d = vel[2]; // Ground downside velocity, m/s

			global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.

			global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally
			global_pos.epv = gps.epv; // Standard deviation of position vertically

			// TODO: implement terrain estimator
			global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
			global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
			// TODO use innovatun consistency check timouts to set this
			global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning

			global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)

			if (_vehicle_global_position_pub == nullptr) {
				_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

			} else {
				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
			}
		}

		// publish estimator status
		struct estimator_status_s status = {};
		status.timestamp = hrt_absolute_time();
		_ekf->get_state_delayed(status.states);
		_ekf->get_covariances(status.covariances);
		//status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value;

		if (_estimator_status_pub == nullptr) {
			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);

		} else {
			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
		}

		// publish estimator innovation data
		struct ekf2_innovations_s innovations = {};
		innovations.timestamp = hrt_absolute_time();
		_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
		_ekf->get_mag_innov(&innovations.mag_innov[0]);
		_ekf->get_heading_innov(&innovations.heading_innov);

		_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
		_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
		_ekf->get_heading_innov_var(&innovations.heading_innov_var);

		if (_estimator_innovations_pub == nullptr) {
			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);

		} else {
			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
		}

		// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
			float decl_deg;
			_ekf->copy_mag_decl_deg(&decl_deg);
			_mag_declination_deg->set(decl_deg);
		}

		_prev_motors_armed = vehicle_control_mode.flag_armed;

	}

	delete ekf2::instance;
	ekf2::instance = nullptr;
}
示例#8
0
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;
}
示例#9
0
void Ekf2::task_main()
{
	// subscribe to relevant topics
	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

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

	// initialise parameter cache
	updateParams();

	// initialize data structures outside of loop
	// because they will else not always be
	// properly populated
	sensor_combined_s sensors = {};
	vehicle_gps_position_s gps = {};
	airspeed_s airspeed = {};
	optical_flow_s optical_flow = {};
	distance_sensor_s range_finder = {};
	vehicle_land_detected_s vehicle_land_detected = {};

	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}

		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
			updateParams();

			// fetch sensor data in next loop
			continue;

		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}

		bool gps_updated = false;
		bool airspeed_updated = false;
		bool optical_flow_updated = false;
		bool range_finder_updated = false;
		bool vehicle_land_detected_updated = false;

		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
		// update all other topics if they have new data
		orb_check(_gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
		}

		orb_check(_airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
		}

		orb_check(_optical_flow_sub, &optical_flow_updated);

		if (optical_flow_updated) {
			orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow);
		}

		orb_check(_range_finder_sub, &range_finder_updated);

		if (range_finder_updated) {
			orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
		}

		// in replay mode we are getting the actual timestamp from the sensor topic
		hrt_abstime now = 0;

		if (_replay_mode) {
			now = sensors.timestamp;

		} else {
			now = hrt_absolute_time();
		}

		// push imu data into estimator
		_ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
				&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);

		// read mag data
		_ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);

		// read baro data
		_ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);

		// read gps data if available
		if (gps_updated) {
			struct gps_message gps_msg = {};
			gps_msg.time_usec = gps.timestamp_position;
			gps_msg.lat = gps.lat;
			gps_msg.lon = gps.lon;
			gps_msg.alt = gps.alt;
			gps_msg.fix_type = gps.fix_type;
			gps_msg.eph = gps.eph;
			gps_msg.epv = gps.epv;
			gps_msg.sacc = gps.s_variance_m_s;
			gps_msg.time_usec_vel = gps.timestamp_velocity;
			gps_msg.vel_m_s = gps.vel_m_s;
			gps_msg.vel_ned[0] = gps.vel_n_m_s;
			gps_msg.vel_ned[1] = gps.vel_e_m_s;
			gps_msg.vel_ned[2] = gps.vel_d_m_s;
			gps_msg.vel_ned_valid = gps.vel_ned_valid;
			gps_msg.nsats = gps.satellites_used;
			//TODO add gdop to gps topic
			gps_msg.gdop = 0.0f;

			_ekf.setGpsData(gps.timestamp_position, &gps_msg);
		}

		// read airspeed data if available
		float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;

		if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) {
			_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
		}

		if (optical_flow_updated) {
			flow_message flow;
			flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
			flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
			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);
			ctrl_state.x_acc = acceleration(0) - accel_bias[0];
			ctrl_state.y_acc = acceleration(1) - accel_bias[1];
			ctrl_state.z_acc = acceleration(2) - accel_bias[2];

			// compute lowpass filtered horizontal acceleration
			acceleration = R_to_body.transpose() * acceleration;
			_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) + acceleration(
						1) * acceleration(1));
			ctrl_state.horz_acc_mag = _acc_hor_filt;

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

			// 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 remaining vehicle attitude data
			att.timestamp = hrt_absolute_time();
			matrix::Euler<float> euler(q);
			att.roll = euler(0);
			att.pitch = euler(1);
			att.yaw = euler(2);

			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 {
				orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
			}

			// generate vehicle local position data
			struct vehicle_local_position_s lpos = {};
			float pos[3] = {};
			float vel[3] = {};

			lpos.timestamp = hrt_absolute_time();

			// Position of body origin in local NED frame
			_ekf.get_position(pos);
			lpos.x = pos[0];
			lpos.y = pos[1];
			lpos.z = pos[2];

			// Velocity of body origin in local NED frame (m/s)
			_ekf.get_velocity(vel);
			lpos.vx = vel[0];
			lpos.vy = vel[1];
			lpos.vz = vel[2];

			// TODO: better status reporting
			lpos.xy_valid = _ekf.local_position_is_valid();
			lpos.z_valid = true;
			lpos.v_xy_valid = _ekf.local_position_is_valid();
			lpos.v_z_valid = true;

			// Position of local NED origin in GPS / WGS84 frame
			struct map_projection_reference_s ekf_origin = {};
			// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
			_ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
			lpos.xy_global = _ekf.global_position_is_valid();
			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 = att.yaw;

			float terrain_vpos;
			lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
			lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
			lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
			lpos.surface_bottom_timestamp	= hrt_absolute_time(); // Time when new bottom surface found

			// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
			Vector3f pos_var, vel_var;
			_ekf.get_pos_var(pos_var);
			_ekf.get_vel_var(vel_var);
			lpos.eph = sqrt(pos_var(0) + pos_var(1));
			lpos.epv = sqrt(pos_var(2));

			// 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 and publish global position data
			struct vehicle_global_position_s global_pos = {};

			if (_ekf.global_position_is_valid()) {
				global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds

				double est_lat, est_lon;
				map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
				global_pos.lat = est_lat; // Latitude in degrees
				global_pos.lon = est_lon; // Longitude in degrees

				global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters

				global_pos.vel_n = vel[0]; // Ground north velocity, m/s
				global_pos.vel_e = vel[1]; // Ground east velocity, m/s
				global_pos.vel_d = vel[2]; // Ground downside velocity, m/s

				global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.

				global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
				global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically

				// TODO: implement terrain estimator
				global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
				global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
				// TODO use innovatun consistency check timouts to set this
				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning

				global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)

				if (_vehicle_global_position_pub == nullptr) {
					_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

				} else {
					orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
				}
			}

		} else if (_replay_mode) {
			// in replay mode we have to tell the replay module not to wait for an update
			// we do this by publishing an attitude with zero timestamp
			struct vehicle_attitude_s att = {};
			att.timestamp = 0;

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

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

		// publish estimator status
		struct estimator_status_s status = {};
		status.timestamp = hrt_absolute_time();
		_ekf.get_state_delayed(status.states);
		_ekf.get_covariances(status.covariances);
		_ekf.get_gps_check_status(&status.gps_check_fail_flags);
		_ekf.get_control_mode(&status.control_mode_flags);
		_ekf.get_filter_fault_status(&status.filter_fault_flags);

		if (_estimator_status_pub == nullptr) {
			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);

		} else {
			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
		}

		// Publish wind estimate
		struct wind_estimate_s wind_estimate = {};
		wind_estimate.timestamp = hrt_absolute_time();
		wind_estimate.windspeed_north = status.states[22];
		wind_estimate.windspeed_east = status.states[23];
		wind_estimate.covariance_north = status.covariances[22];
		wind_estimate.covariance_east = status.covariances[23];

		if (_wind_pub == nullptr) {
			_wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate);

		} else {
			orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate);
		}

		// publish estimator innovation data
		struct ekf2_innovations_s innovations = {};
		innovations.timestamp = hrt_absolute_time();
		_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
		_ekf.get_mag_innov(&innovations.mag_innov[0]);
		_ekf.get_heading_innov(&innovations.heading_innov);
		_ekf.get_airspeed_innov(&innovations.airspeed_innov);
		_ekf.get_flow_innov(&innovations.flow_innov[0]);
		_ekf.get_hagl_innov(&innovations.hagl_innov);

		_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
		_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
		_ekf.get_heading_innov_var(&innovations.heading_innov_var);
		_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
		_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
		_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);

		if (_estimator_innovations_pub == nullptr) {
			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);

		} else {
			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
		}

		// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
		if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
			float decl_deg;
			_ekf.copy_mag_decl_deg(&decl_deg);
			_mag_declination_deg.set(decl_deg);
		}
		_prev_landed = vehicle_land_detected.landed;

		// publish replay message if in replay mode
		bool publish_replay_message = (bool)_param_record_replay_msg.get();

		if (publish_replay_message) {
			struct ekf2_replay_s replay = {};
			replay.time_ref = now;
			replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
			replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
			replay.baro_timestamp = sensors.baro_timestamp[0];
			memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
			       sizeof(replay.accelerometer_integral_m_s));
			memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
			replay.baro_alt_meter = sensors.baro_alt_meter[0];

			// only write gps data if we had a gps update.
			if (gps_updated) {
				replay.time_usec = gps.timestamp_position;
				replay.time_usec_vel = gps.timestamp_velocity;
				replay.lat = gps.lat;
				replay.lon = gps.lon;
				replay.alt = gps.alt;
				replay.fix_type = gps.fix_type;
				replay.nsats = gps.satellites_used;
				replay.eph = gps.eph;
				replay.epv = gps.epv;
				replay.sacc = gps.s_variance_m_s;
				replay.vel_m_s = gps.vel_m_s;
				replay.vel_n_m_s = gps.vel_n_m_s;
				replay.vel_e_m_s = gps.vel_e_m_s;
				replay.vel_d_m_s = gps.vel_d_m_s;
				replay.vel_ned_valid = gps.vel_ned_valid;

			} else {
				// this will tell the logging app not to bother logging any gps replay data
				replay.time_usec = 0;
			}

			if (optical_flow_updated) {
				replay.flow_timestamp = optical_flow.timestamp;
				replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral;
				replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral;
				replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
				replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
				replay.flow_time_integral = optical_flow.integration_timespan;
				replay.flow_quality = optical_flow.quality;

			} else {
				replay.flow_timestamp = 0;
			}

			if (range_finder_updated) {
				replay.rng_timestamp = range_finder.timestamp;
				replay.range_to_ground = range_finder.current_distance;

			} else {
				replay.rng_timestamp = 0;
			}

			if (airspeed_updated) {
				replay.asp_timestamp = airspeed.timestamp;
				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
				replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
				replay.air_temperature_celsius = airspeed.air_temperature_celsius;
				replay.confidence = airspeed.confidence;

			} else {
				replay.asp_timestamp = 0;
			}


			if (_replay_pub == nullptr) {
				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);

			} else {
				orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
			}
		}
	}

	delete ekf2::instance;
	ekf2::instance = nullptr;
}
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;

	// Update all the counters.
	perf_set_count(_read_counter, data.read_counter);
	perf_set_count(_error_counter, data.error_counter);
	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);

	if (_mag_enabled) {
		perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
	}

	perf_begin(_publish_perf);

	// TODO: get these right
	gyro_report.scaling = -1.0f;
	gyro_report.range_rad_s = -1.0f;
	gyro_report.device_id = m_id.dev_id;

	accel_report.scaling = -1.0f;
	accel_report.range_m_s2 = -1.0f;
	accel_report.device_id = m_id.dev_id;

	if (_mag_enabled) {
		mag_report.timestamp = accel_report.timestamp;
		mag_report.is_external = false;

		mag_report.scaling = -1.0f;
		mag_report.range_ga = -1.0f;
		mag_report.device_id = m_id.dev_id;

		xraw_f = data.mag_ga_x;
		yraw_f = data.mag_ga_y;
		zraw_f = data.mag_ga_z;

		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
		mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
		mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
		mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]

		mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
		mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
		mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;
	}

	// TODO: when is this ever blocked?
	if (!(m_pub_blocked)) {

		if (_gyro_topic != nullptr) {
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}

		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}

		if (_mag_enabled) {

			if (_mag_topic == nullptr) {
				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
								 &_mag_orb_class_instance, ORB_PRIO_LOW);

			} else {
				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
			}
		}

		// Report if there are high vibrations, every 10 times it happens.
		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);

		// Report every 5s.
		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);

		if (threshold_reached && due_to_report) {
			mavlink_log_critical(&_mavlink_log_pub,
					     "High accelerations, range exceeded %llu times",
					     data.accel_range_hit_counter);

			_last_accel_range_hit_time = hrt_absolute_time();
			_last_accel_range_hit_count = data.accel_range_hit_counter;
		}
	}

	perf_end(_publish_perf);

	// TODO: check the return codes of this function
	return 0;
};
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;

		int best_gyro = 0;
		int best_accel = 0;
		int best_mag = 0;

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

			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {

				/* ignore empty fields */
				if (sensors.gyro_timestamp[i] > 0) {

					float gyro[3];

					for (unsigned j = 0; j < 3; j++) {
						if (sensors.gyro_integral_dt[i] > 0) {
							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);

						} else {
							/* fall back to angular rate */
							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
						}
					}

					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
				}

				/* ignore empty fields */
				if (sensors.accelerometer_timestamp[i] > 0) {
					_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
							 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
				}

				/* ignore empty fields */
				if (sensors.magnetometer_timestamp[i] > 0) {
					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
				}
			}

			// Get best measurement values
			hrt_abstime curr_time = hrt_absolute_time();
			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
			_mag.set(_voter_mag.get_best(curr_time, &best_mag));

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

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

			_data_good = true;

			if (!_failsafe) {
				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;

#ifdef __PX4_POSIX
				perf_end(_perf_accel);
				perf_end(_perf_mpu);
				perf_end(_perf_mag);
#endif

				if (_voter_gyro.failover_count() > 0) {
					_failsafe = true;
					flags = _voter_gyro.failover_state();
					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
									  _voter_gyro.failover_index(),
									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
				}

				if (_voter_accel.failover_count() > 0) {
					_failsafe = true;
					flags = _voter_accel.failover_state();
					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
									  _voter_accel.failover_index(),
									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
				}

				if (_voter_mag.failover_count() > 0) {
					_failsafe = true;
					flags = _voter_mag.failover_state();
					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
									  _voter_mag.failover_index(),
									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
				}

				if (_failsafe) {
					mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
				}
			}

			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
						    _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
						    _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {

				if (_vibration_warning_timestamp == 0) {
					_vibration_warning_timestamp = curr_time;

				} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
					_vibration_warning = true;
					mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
									 (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
									 (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
									 (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
				}

			} else {
				_vibration_warning_timestamp = 0;
			}
		}

		// 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
			_vision_hdg = Rvis.transposed() * v;
		}

		if (mocap_updated) {
			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
			math::Quaternion q(_mocap.q);
			math::Matrix<3, 3> Rmoc = q.to_dcm();

			math::Vector<3> v(1.0f, 0.0f, 0.4f);

			// Rmoc is Rwr (robot respect to world) while v is respect to world.
			// Hence Rmoc must be transposed having (Rwr)' * Vw
			// Rrw * Vw = vn. This way we have consistency
			_mocap_hdg = Rmoc.transposed() * v;
		}

		// Update airspeed
		bool airspeed_updated = false;
		orb_check(_airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
		}

		// Check for timeouts on data
		if (_ext_hdg_mode == 1) {
			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);

		} else if (_ext_hdg_mode == 2) {
			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
		}

		bool gpos_updated;
		orb_check(_global_pos_sub, &gpos_updated);

		if (gpos_updated) {
			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);

			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
				/* set magnetic declination automatically */
				update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
			}
		}

		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
			/* position data is actual */
			if (gpos_updated) {
				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);

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

		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);
		}
	}
}
示例#12
0
void
FXOS8700CQ::measure()
{
	/* status register and data as read back from the device */

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

	accel_report accel_report;

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

	check_registers();

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

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

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

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

	accel_report.timestamp = hrt_absolute_time();

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

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

	accel_report.temperature = _last_temperature;

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

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

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

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

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

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

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	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
			_vision_hdg = Rvis.transposed() * v;
		}

		if (mocap_updated) {
			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
			math::Quaternion q(_mocap.q);
			math::Matrix<3, 3> Rmoc = q.to_dcm();

			math::Vector<3> v(1.0f, 0.0f, 0.4f);

			// Rmoc is Rwr (robot respect to world) while v is respect to world.
			// Hence Rmoc must be transposed having (Rwr)' * Vw
			// Rrw * Vw = vn. This way we have consistency
			_mocap_hdg = Rmoc.transposed() * v;
		}

		// Update airspeed
		bool airspeed_updated = false;
		orb_check(_airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
		}

		// Check for timeouts on data
		if (_ext_hdg_mode == 1) {
			_ext_hdg_good = _vision.timestamp > 0 && (hrt_elapsed_time(&_vision.timestamp) < 500000);

		} else if (_ext_hdg_mode == 2) {
			_ext_hdg_good = _mocap.timestamp > 0 && (hrt_elapsed_time(&_mocap.timestamp) < 500000);
		}

		bool gpos_updated;
		orb_check(_global_pos_sub, &gpos_updated);

		if (gpos_updated) {
			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);

			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
				/* set magnetic declination automatically */
				update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
			}
		}

		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
			/* position data is actual */
			if (gpos_updated) {
				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);

				/* 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.rollspeed = _rates(0);
		att.pitchspeed = _rates(1);
		att.yawspeed = _rates(2);

		memcpy(&att.q[0], _q.data, sizeof(att.q));

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

			ctrl_state.x_acc = _accel(0);
			ctrl_state.y_acc = _accel(1);
			ctrl_state.z_acc = _accel(2);

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

			ctrl_state.pitch_rate = _rates(1);

			ctrl_state.yaw_rate = _rates(2);

			ctrl_state.airspeed_valid = false;

			if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
				// use measured airspeed
				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 if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
				// use estimated body velocity as airspeed estimate
				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
					ctrl_state.airspeed_valid = true;
				}

			} else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
				// do nothing, airspeed has been declared as non-valid above, controllers
				// will handle this assuming always trim airspeed
			}

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

			/* the instance count is not used here */
			//int est_inst;
			/* publish to control state topic */
			// TODO handle attitude states in position estimators instead so we can publish all data at once
			// or we need to enable more thatn just one estimator_status topic
			// orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
		}
	}

#ifdef __PX4_POSIX
	perf_end(_perf_accel);
	perf_end(_perf_mpu);
	perf_end(_perf_mag);
#endif

	orb_unsubscribe(_sensors_sub);
	orb_unsubscribe(_vision_sub);
	orb_unsubscribe(_mocap_sub);
	orb_unsubscribe(_airspeed_sub);
	orb_unsubscribe(_params_sub);
	orb_unsubscribe(_global_pos_sub);
}
示例#14
0
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);
}
示例#15
0
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);
}
示例#16
0
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);
	gyro_report.y_integral = gval_integrated(1);
	gyro_report.z_integral = gval_integrated(2);

	gyro_report.scaling = _gyro_range_scale;

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


	_reports->force(&gyro_report);

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

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

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
示例#17
0
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:
		// Normal Operation. Good Data Packet
		break;

	case 1:
		// Reserved
		return -EAGAIN;

	case 2:
		// Stale Data. Data has been fetched since last measurement cycle.
		return -EAGAIN;

	case 3:
		// Fault Detected
		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;

	// dT max is almost certainly an invalid reading
	if (dT_raw == 2047) {
		perf_count(_comms_errors);
		return -EAGAIN;
	}

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

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

	struct differential_pressure_s report;

	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) - _diff_pres_offset;
	report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
	report.device_id = _device_id.devid;

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

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}