diff options
Diffstat (limited to 'src/modules/sdlog2/sdlog2.c')
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 18 |
1 files changed, 18 insertions, 0 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 365890b35..bc8c0f0db 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -83,6 +83,7 @@ #include <uORB/topics/rc_channels.h> #include <uORB/topics/esc_status.h> #include <uORB/topics/telemetry_status.h> +#include <uORB/topics/estimator_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -794,6 +795,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; + struct estimator_status_report estimator_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -825,6 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; + struct log_ESTM_s log_ESTM; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +858,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int battery_sub; int telemetry_sub; int range_finder_sub; + int estimator_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -879,6 +883,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); thread_running = true; @@ -1243,6 +1248,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(DIST); } + /* --- ESTIMATOR STATUS --- */ + if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + log_msg.msg_type = LOG_ESTM_MSG; + unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); + memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); + memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); + log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; + log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; + log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; + log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; + LOGBUFFER_WRITE_AND_COUNT(ESTM); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ |