From 6b5e77250f3144180cb7a3d90ce8c0906b3f77f4 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 9 May 2016 15:42:26 +0200 Subject: [PATCH] ekf2: Added airspeed to rpl logging --- msg/ekf2_replay.msg | 9 +++++++++ src/modules/ekf2/ekf2_main.cpp | 13 +++++++++++++ src/modules/sdlog2/sdlog2.c | 14 ++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 13 +++++++++++++ 4 files changed, 49 insertions(+) diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg index d41ef166d1..e3127ef0e9 100644 --- a/msg/ekf2_replay.msg +++ b/msg/ekf2_replay.msg @@ -6,6 +6,7 @@ uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us uint64 baro_timestamp # timestamp of barometer measurement in us uint64 rng_timestamp # timestamp of range finder measurement in us uint64 flow_timestamp # timestamp of optical flow measurement in us +uint64 asp_timestamp # timestamp of airspeed measurement in us float32[3] gyro_integral_rad # integrated gyro vector in rad float32[3] accelerometer_integral_m_s # integrated accelerometer vector in m/s @@ -37,3 +38,11 @@ float32[2] flow_pixel_integral # integrated optical flow rate around x and y ax float32[2] flow_gyro_integral # integrated gyro rate around x and y axes (rad) uint32 flow_time_integral # integration timespan (usec) uint8 flow_quality # Quality of accumulated optical flow data (0 - 255) + +# airspeed +float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown +float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown +float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown +float32 confidence # confidence value from 0 to 1 for this sensor + diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 051b0f8b49..dd30bb54e4 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 19785078ae..5c2959e7c1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3def0285c1..67e4b5ab97 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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 */