fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Added airspeed to rpl logging

Browse Source
This commit is contained in:
CarlOlsson
2016-05-10 21:32:55 +02:00
committed by Lorenz Meier
parent f4c8bd9be3
commit 6b5e77250f
4 changed files with 49 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+13
View File
@@ -848,6 +848,19 @@ void Ekf2::task_main()
replay.rng_timestamp = 0;
}
if (airspeed_updated) {
replay.asp_timestamp = airspeed.timestamp;
replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
replay.air_temperature_celsius = airspeed.air_temperature_celsius;
replay.confidence = airspeed.confidence;
} else {
replay.asp_timestamp = 0;
}
if (_replay_pub == nullptr) {
_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
src/modules/sdlog2/sdlog2.c
+14
View File
@@ -1230,6 +1230,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST6_s log_INO3;
struct log_RPL3_s log_RPL3;
struct log_RPL4_s log_RPL4;
struct log_RPL6_s log_RPL6;
struct log_LAND_s log_LAND;
} body;
} log_msg = {
@@ -1532,6 +1533,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(RPL4);
}
if (buf.replay.asp_timestamp > 0) {
log_msg.msg_type = LOG_RPL6_MSG;
log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
log_msg.body.log_RPL6.confidence = buf.replay.confidence;
LOGBUFFER_WRITE_AND_COUNT(RPL6);
}
} else { /* !record_replay_log */
// we poll on sensor combined, so we know it has updated just now
src/modules/sdlog2/sdlog2_messages.h
+13
View File
@@ -576,6 +576,18 @@ struct log_RPL4_s {
float range_to_ground;
};
/* --- EKF2 REPLAY Part 4 --- */
#define LOG_RPL6_MSG 59
struct log_RPL6_s {
uint64_t timestamp;
float indicated_airspeed_m_s;
float true_airspeed_m_s;
float true_airspeed_unfiltered_m_s;
float air_temperature_celsius;
float confidence;
};
/* --- CAMERA TRIGGER --- */
#define LOG_CAMT_MSG 55
@@ -673,6 +685,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
LOG_FORMAT(LAND, "B", "Landed"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */