fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 16:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2: Update to new sensors combined topic

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-30 00:11:23 +02:00
parent ae569ba543
commit ad14b471e3
2 changed files with 84 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/module.mk
+1 -1
View File
@@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
ifeq ($(PX4_TARGET_OS),nuttx)
EXTRACFLAGS = -Wframe-larger-than=1400
EXTRACFLAGS = -Wframe-larger-than=1600
endif
src/modules/sdlog2/sdlog2.c
+83 -148
View File
@@ -1284,18 +1284,11 @@ int sdlog2_thread_main(int argc, char *argv[])
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
hrt_abstime gyro_timestamp = 0;
hrt_abstime accelerometer_timestamp = 0;
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
hrt_abstime barometer1_timestamp = 0;
hrt_abstime gyro1_timestamp = 0;
hrt_abstime accelerometer1_timestamp = 0;
hrt_abstime magnetometer1_timestamp = 0;
hrt_abstime gyro2_timestamp = 0;
hrt_abstime accelerometer2_timestamp = 0;
hrt_abstime magnetometer2_timestamp = 0;
hrt_abstime gyro_timestamp[3] = {0, 0, 0};
hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
hrt_abstime barometer_timestamp[3] = {0, 0, 0};
hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0};
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
@@ -1443,144 +1436,86 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
bool write_SENS = false;
bool write_SENS1 = false;
if (buf.sensor.timestamp != gyro_timestamp) {
gyro_timestamp = buf.sensor.timestamp;
write_IMU = true;
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);
}
}
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true;
}
if (buf.sensor.baro_timestamp != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true;
}
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
write_SENS = true;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
barometer1_timestamp = buf.sensor.baro1_timestamp;
write_SENS1 = true;
}
if (write_SENS1) {
log_msg.msg_type = LOG_AIR1_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
// XXX moving to AIR0-AIR2 instead of SENS
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
gyro1_timestamp = buf.sensor.gyro1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
write_IMU1 = true;
}
if (write_IMU1) {
log_msg.msg_type = LOG_IMU1_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
gyro2_timestamp = buf.sensor.gyro2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
write_IMU2 = true;
}
if (write_IMU2) {
log_msg.msg_type = LOG_IMU2_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp;
log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp;
log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp;
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
}
/* --- ATTITUDE --- */