fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 12:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2: Add support for up to three IMU sensors

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-15 23:19:45 +02:00
parent 64e33b8896
commit b8b6974e22
2 changed files with 73 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+66
View File
@@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime magnetometer_timestamp = 0; hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0; hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0; hrt_abstime differential_pressure_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;
/* initialize calculated mean SNR */ /* initialize calculated mean SNR */
float snr_mean = 0.0f; float snr_mean = 0.0f;
@@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SENSOR COMBINED --- */ /* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false; bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
bool write_SENS = false; bool write_SENS = false;
if (buf.sensor.timestamp != gyro_timestamp) { if (buf.sensor.timestamp != gyro_timestamp) {
@@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(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];
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];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
} }
/* --- ATTITUDE --- */ /* --- ATTITUDE --- */
src/modules/sdlog2/sdlog2_messages.h
+7 -3
View File
@@ -73,6 +73,8 @@ struct log_ATSP_s {
/* --- IMU - IMU SENSORS --- */ /* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4 #define LOG_IMU_MSG 4
#define LOG_IMU1_MSG 22
#define LOG_IMU2_MSG 23
struct log_IMU_s { struct log_IMU_s {
float acc_x; float acc_x;
float acc_y; float acc_y;
@@ -276,8 +278,8 @@ struct log_DIST_s {
uint8_t flags; uint8_t flags;
}; };
// ID 22 available /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
// ID 23 available
/* --- PWR - ONBOARD POWER SYSTEM --- */ /* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24 #define LOG_PWR_MSG 24
@@ -420,7 +422,9 @@ static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */ /* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),