fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 15:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sdlog2 : field for timesync offset

Browse Source
This commit is contained in:
Mohammed Kabir
2015-04-14 13:14:15 +05:30
parent 66e6938c6d
commit 2f164ca345
2 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+12
View File
@@ -99,6 +99,7 @@
#include <uORB/topics/wind_estimate.h> #include <uORB/topics/wind_estimate.h>
#include <uORB/topics/encoders.h> #include <uORB/topics/encoders.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/time_offset.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@@ -1027,6 +1028,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct wind_estimate_s wind_estimate; struct wind_estimate_s wind_estimate;
struct encoders_s encoders; struct encoders_s encoders;
struct vtol_vehicle_status_s vtol_status; struct vtol_vehicle_status_s vtol_status;
struct time_offset_s time_offset;
} buf; } buf;
memset(&buf, 0, sizeof(buf)); memset(&buf, 0, sizeof(buf));
@@ -1071,6 +1073,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_TECS_s log_TECS; struct log_TECS_s log_TECS;
struct log_WIND_s log_WIND; struct log_WIND_s log_WIND;
struct log_ENCD_s log_ENCD; struct log_ENCD_s log_ENCD;
struct log_TSYN_s log_TSYN;
} body; } body;
} log_msg = { } log_msg = {
LOG_PACKET_HEADER_INIT(0) LOG_PACKET_HEADER_INIT(0)
@@ -1111,6 +1114,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int servorail_status_sub; int servorail_status_sub;
int wind_sub; int wind_sub;
int encoders_sub; int encoders_sub;
int tsync_sub;
} subs; } subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1142,6 +1146,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
subs.tsync_sub = orb_subscribe(ORB_ID(time_offset));
/* we need to rate-limit wind, as we do not need the full update rate */ /* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90); orb_set_interval(subs.wind_sub, 90);
@@ -1821,6 +1826,13 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ENCD); LOGBUFFER_WRITE_AND_COUNT(ENCD);
} }
/* --- TIMESYNC OFFSET --- */
if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) {
log_msg.msg_type = LOG_TSYN_MSG;
log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns;
LOGBUFFER_WRITE_AND_COUNT(TSYN);
}
/* signal the other thread new data, but not yet unlock */ /* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */ /* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+7
View File
@@ -449,6 +449,12 @@ struct log_VTOL_s {
float airspeed_tot; float airspeed_tot;
}; };
/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
#define LOG_TSYN_MSG 43
struct log_TSYN_s {
uint64_t time_offset;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/ /********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */ /* --- TIME - TIME STAMP --- */
@@ -517,6 +523,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
/* system-level messages, ID >= 0x80 */ /* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */ /* FMT: don't write format of format message, it's useless */