dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sdlog2: Logging wind estimate at low rate

This commit is contained in:
Lorenz Meier 2014-06-01 15:35:26 +02:00
parent 50342f9661
commit ef6af184aa
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sdlog2/sdlog2.c
View File

@ -87,6 +87,7 @@
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -943,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct tecs_status_s tecs_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
struct wind_estimate_s wind_estimate;
} buf;
memset(&buf, 0, sizeof(buf));
@ -982,6 +984,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GS1A_s log_GS1A;
struct log_GS1B_s log_GS1B;
struct log_TECS_s log_TECS;
struct log_WIND_s log_WIND;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -1016,6 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int tecs_status_sub;
int system_power_sub;
int servorail_status_sub;
int wind_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@ -1044,6 +1048,9 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
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);
thread_running = true;
@ -1512,6 +1519,16 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(TECS);
}
/* --- WIND ESTIMATE --- */
if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
log_msg.msg_type = LOG_WIND_MSG;
log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
LOGBUFFER_WRITE_AND_COUNT(WIND);
}
/* 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 */

10
src/modules/sdlog2/sdlog2_messages.h
View File

@ -366,6 +366,15 @@ struct log_TECS_s {
uint8_t mode;
};
/* --- WIND - WIND ESTIMATE --- */
#define LOG_WIND_MSG 31
struct log_WIND_s {
float x;
float y;
float cov_x;
float cov_y;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -422,6 +431,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
LOG_FORMAT(WIND, "fff", "X,Y,Cov"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */