diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-02 17:09:36 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-02 17:09:36 +0400 |
commit | 5c53797c1710d49d1df87515509ad81ea5367a21 (patch) | |
tree | e04a7ed4e712354eed8c2567176a72409310ff80 /src/modules/sdlog2/sdlog2.c | |
parent | 93617c4073d560ec2a804d728a2830534a74a50a (diff) | |
parent | 0205eebaa63016b3cf4bb03a5af230554d4a581b (diff) | |
download | px4-firmware-5c53797c1710d49d1df87515509ad81ea5367a21.tar.gz px4-firmware-5c53797c1710d49d1df87515509ad81ea5367a21.tar.bz2 px4-firmware-5c53797c1710d49d1df87515509ad81ea5367a21.zip |
Merge branch 'master' into mpc_local_pos
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 */ |