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

Log extended estimator status

This commit is contained in:
Lorenz Meier 2014-06-04 09:51:40 +02:00
parent 7a6d33ba47
commit 77a4711ff9
2 changed files with 36 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -976,7 +976,8 @@ 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;
struct log_EST0_s log_EST0;
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_GS0A_s log_GS0A;
@ -1489,15 +1490,21 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- 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);
log_msg.msg_type = LOG_EST0_MSG;
unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
LOGBUFFER_WRITE_AND_COUNT(EST0);
log_msg.msg_type = LOG_EST1_MSG;
unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s);
memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
LOGBUFFER_WRITE_AND_COUNT(EST1);
}
/* --- TECS STATUS --- */

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

@ -288,15 +288,7 @@ struct log_TELE_s {
uint8_t txbuf;
};
/* --- ESTM - ESTIMATOR STATUS --- */
#define LOG_ESTM_MSG 23
struct log_ESTM_s {
float s[10];
uint8_t n_states;
uint8_t states_nan;
uint8_t covariance_nan;
uint8_t kalman_gain_nan;
};
// ID 23 available
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
@ -375,6 +367,22 @@ struct log_WIND_s {
float cov_y;
};
/* --- EST0 - ESTIMATOR STATUS --- */
#define LOG_EST0_MSG 32
struct log_EST0_s {
float s[12];
uint8_t n_states;
uint8_t nan_flags;
uint8_t health_flags;
uint8_t timeout_flags;
};
/* --- EST1 - ESTIMATOR STATUS --- */
#define LOG_EST1_MSG 33
struct log_EST1_s {
float s[16];
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@ -423,7 +431,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
LOG_FORMAT(EST0, "ffffffffffffffBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,nStat,flagsNaN"),
LOG_FORMAT(EST1, "ffffffffffffffff", "s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),