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

Encoder logging support.

Browse Source
This commit is contained in:
James Goppert
2014-11-02 12:49:43 -05:00
parent acb739655d
commit baba157785
2 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+15 -1
View File
@@ -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>
@@ -954,6 +955,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 +998,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_ENCODERS_s log_ENCODERS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1033,6 +1036,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 +1068,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 */
@@ -1667,6 +1671,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_ENCODERS_MSG;
log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0];
log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0];
log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1];
log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1];
LOGBUFFER_WRITE_AND_COUNT(ENCODERS);
}
/* 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 */