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