fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2: Ensure sensor combined gets written

Browse Source
This commit is contained in:
Lorenz Meier 2016-02-27 14:58:53 +01:00
parent df4d63dd07
commit 3fa3158ca2
1 changed files with 81 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
src/modules/sdlog2/sdlog2.c
View File

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