zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sdlog2: Update to new sensors combined topic

sbg
Lorenz Meier 10 years ago
parent
ae569ba543
commit
ad14b471e3
2 changed files with 75 additions and 140 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/modules/sdlog2/module.mk
  2. 175
      src/modules/sdlog2/sdlog2.c

2
src/modules/sdlog2/module.mk
Unescape View File

@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200 @@ -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

175
src/modules/sdlog2/sdlog2.c
Unescape View File

@ -1284,18 +1284,11 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -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[]) @@ -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)) {
for (unsigned i = 0; i < 3; i++) {
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;
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 != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
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 != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
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 != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
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 != differential_pressure_timestamp) {
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
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;
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;
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;
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) {
break;
case 1:
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;
break;
case 2:
continue;
break;
}
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
write_IMU2 = true;
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 (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 --- */

Write Preview
Loading…
Cancel
Save