Exemple #1
0
void
l_sensor_combined(const struct listener *l)
{
	struct sensor_combined_s raw;

	/* copy sensors raw data into local buffer */
	orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);

	last_sensor_timestamp = raw.timestamp;

	/* mark individual fields as changed */
	uint16_t fields_updated = 0;
	static unsigned accel_counter = 0;
	static unsigned gyro_counter = 0;
	static unsigned mag_counter = 0;
	static unsigned baro_counter = 0;

	if (accel_counter != raw.accelerometer_counter) {
		/* mark first three dimensions as changed */
		fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
		accel_counter = raw.accelerometer_counter;
	}

	if (gyro_counter != raw.gyro_counter) {
		/* mark second group dimensions as changed */
		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
		gyro_counter = raw.gyro_counter;
	}

	if (mag_counter != raw.magnetometer_counter) {
		/* mark third group dimensions as changed */
		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
		mag_counter = raw.magnetometer_counter;
	}

	if (baro_counter != raw.baro_counter) {
		/* mark last group dimensions as changed */
		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
		baro_counter = raw.baro_counter;
	}

	if (gcs_link)
		mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
					     raw.magnetometer_ga[0],
					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
					     raw.baro_pres_mbar, 0 /* no diff pressure yet */,
					     raw.baro_alt_meter, raw.baro_temp_celcius,
					     fields_updated);

	sensors_raw_counter++;
}
	void send(const hrt_abstime t)
	{
		struct sensor_combined_s sensor;

		if (sensor_sub->update(&sensor_time, &sensor)) {
			uint16_t fields_updated = 0;

			if (accel_timestamp != sensor.accelerometer_timestamp) {
				/* mark first three dimensions as changed */
				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
				accel_timestamp = sensor.accelerometer_timestamp;
			}

			if (gyro_timestamp != sensor.timestamp) {
				/* mark second group dimensions as changed */
				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
				gyro_timestamp = sensor.timestamp;
			}

			if (mag_timestamp != sensor.magnetometer_timestamp) {
				/* mark third group dimensions as changed */
				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
				mag_timestamp = sensor.magnetometer_timestamp;
			}

			if (baro_timestamp != sensor.baro_timestamp) {
				/* mark last group dimensions as changed */
				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
				baro_timestamp = sensor.baro_timestamp;
			}

			mavlink_msg_highres_imu_send(_channel,
						     sensor.timestamp,
						     sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
						     sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
						     sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
						     sensor.baro_pres_mbar, sensor.differential_pressure_pa,
						     sensor.baro_alt_meter, sensor.baro_temp_celcius,
						     fields_updated);
		}
	}