diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 90b86d271f..a99d3441b2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1496,6 +1496,87 @@ int sdlog2_thread_main(int argc, char *argv[]) } if (!record_replay_log) { + + /* we poll on sensor combined, so we know it has updated just now */ + for (unsigned i = 0; i < 3; i++) { + bool write_IMU = false; + bool write_SENS = false; + + if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { + gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { + accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { + magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { + barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; + write_SENS = true; + } + + if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { + differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; + write_SENS = true; + } + + if (write_IMU) { + switch (i) { + case 0: + log_msg.msg_type = LOG_IMU_MSG; + break; + case 1: + log_msg.msg_type = LOG_IMU1_MSG; + break; + case 2: + log_msg.msg_type = LOG_IMU2_MSG; + break; + } + + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (write_SENS) { + switch (i) { + case 0: + log_msg.msg_type = LOG_SENS_MSG; + break; + case 1: + log_msg.msg_type = LOG_AIR1_MSG; + break; + case 2: + continue; + break; + } + + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + } + /* --- VTOL VEHICLE STATUS --- */ if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { log_msg.msg_type = LOG_VTOL_MSG; @@ -1573,89 +1654,6 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- SENSOR COMBINED --- */ - if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { - - for (unsigned i = 0; i < 3; i++) { - bool write_IMU = false; - bool write_SENS = false; - - if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { - gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; - write_IMU = true; - } - - if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { - accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; - write_IMU = true; - } - - if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { - magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; - write_IMU = true; - } - - if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { - barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { - differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; - write_SENS = true; - } - - if (write_IMU) { - switch (i) { - case 0: - log_msg.msg_type = LOG_IMU_MSG; - break; - case 1: - log_msg.msg_type = LOG_IMU1_MSG; - break; - case 2: - log_msg.msg_type = LOG_IMU2_MSG; - break; - } - - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0]; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0]; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0]; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { - switch (i) { - case 0: - log_msg.msg_type = LOG_SENS_MSG; - break; - case 1: - log_msg.msg_type = LOG_AIR1_MSG; - break; - case 2: - continue; - break; - } - - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - } - } - /* --- ATTITUDE SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) { log_msg.msg_type = LOG_ATSP_MSG;