diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-26 07:43:19 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-26 07:43:19 +0100 |
commit | cbcd1ea1d143dfddd2331fb14d17d7be48fa8b6f (patch) | |
tree | 5be46ffd492dd3980ccfb3b7303238d22a22ff6c /src/modules/sdlog2/sdlog2.c | |
parent | 739c407cfd14d065b104977924875b3ee40d5e25 (diff) | |
parent | 4724c050478900a0b0b760877f1613e43c0aa97c (diff) | |
download | px4-firmware-cbcd1ea1d143dfddd2331fb14d17d7be48fa8b6f.tar.gz px4-firmware-cbcd1ea1d143dfddd2331fb14d17d7be48fa8b6f.tar.bz2 px4-firmware-cbcd1ea1d143dfddd2331fb14d17d7be48fa8b6f.zip |
Merged PX4Flow driver changes
Diffstat (limited to 'src/modules/sdlog2/sdlog2.c')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 23 |
1 files changed, 22 insertions, 1 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 54c7b0c83..0b6861d2a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include <uORB/topics/system_power.h> #include <uORB/topics/servorail_status.h> #include <uORB/topics/wind_estimate.h> +#include <uORB/topics/encoders.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -495,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -552,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -585,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } @@ -954,6 +962,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -996,6 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1033,6 +1043,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1064,7 +1075,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); - + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); /* add new topics HERE */ @@ -1670,6 +1681,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); + } + /* 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 */ |