From e87460b9f4d2a5dde2c0acd07fe3c727007b27c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 24 May 2014 18:07:46 +0200 Subject: sdlog: log tecs status messages --- src/modules/sdlog2/sdlog2_messages.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index f4d88f079..93ed0cdfc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,6 +346,24 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; +/* --- TECS - TECS STAUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float airspeedSp; + float airspeed; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -401,6 +419,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), 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, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From bb0cae56a0bca879bd3f1720a92d1bf0ca03eda2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 May 2014 08:42:08 +0200 Subject: fix typo in comment --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93ed0cdfc..8a0a0ad45 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,7 +346,7 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; -/* --- TECS - TECS STAUS --- */ +/* --- TECS - TECS STATUS --- */ #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; -- cgit v1.2.3 From ca6e309ba05c6793543c55d9507971df6f0721c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 19:43:58 +0200 Subject: sdlog2: log mtecs mode --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e1f3490a9..17461f587 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1508,6 +1508,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8a0a0ad45..83832580c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -362,6 +362,8 @@ struct log_TECS_s { float totalEnergyRate; float energyDistributionRateSp; float energyDistributionRate; + + uint8_t mode; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -419,7 +421,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), 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, "ffffffffffff", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"), + LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From ef6af184aa7858a9124de13c958f02b140b0f712 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:35:26 +0200 Subject: sdlog2: Logging wind estimate at low rate --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 27 insertions(+) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 17461f587..c19579f0f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -87,6 +87,7 @@ #include #include #include +#include #include #include @@ -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 */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 83832580c..a6eb7a5bd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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 */ -- cgit v1.2.3 From 41a763a50e99880147a2e4f5a6988299f8fd6dac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:39:38 +0200 Subject: Fix log format specifier --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a6eb7a5bd..a874351b3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +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"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 77a4711ff992b4cbda03f539dd2e7bd927b76e95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 09:51:40 +0200 Subject: Log extended estimator status --- src/modules/sdlog2/sdlog2.c | 27 +++++++++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 29 +++++++++++++++++++---------- 2 files changed, 36 insertions(+), 20 deletions(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c19579f0f..5e6946658 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a874351b3..a3c9d2440 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"), -- cgit v1.2.3 From fffd3c5d1242dd908b784ece996513b034e98709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Jun 2014 10:32:46 +0200 Subject: Fix format specifier --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a3c9d2440..81bbbe560 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ 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(EST0, "ffffffffffffffBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,nStat,flagsNaN"), + LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), 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"), -- cgit v1.2.3 From ba36890ef32efd6b51490237f12378dfe1738353 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Jun 2014 16:59:18 +0200 Subject: sdlog2: Fix estimator state labels --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 81bbbe560..9a26e1c51 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -432,7 +432,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), - LOG_FORMAT(EST1, "ffffffffffffffff", "s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), 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"), -- cgit v1.2.3 From f9946c98a809d18e0a037ee45f39195fd92c62fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Jun 2014 21:03:27 +0200 Subject: mtecs: filter airspeed --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 12 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 6 ++++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 32 insertions(+), 13 deletions(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 5894333f3..03353cbc1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), @@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); + /* Filter arispeed */ + float airspeedFiltered = _airspeedLowpass.update(airspeed); + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ - float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); /* Debug output */ if (_counter % 10 == 0) { - debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); } /* Write part of the status message */ @@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; + _status.airspeedFiltered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, accelerationLongitudinalSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { return -1; } /* time measurement */ @@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float flightPathAngleError = flightPathAngleSp - flightPathAngle; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { - airspeedDerivative = _airspeedDerivative.update(airspeed); + airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); } float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; @@ -186,7 +194,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) { + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 0369640f2..c102f5dee 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -76,7 +76,7 @@ public: /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* @@ -90,9 +90,10 @@ public: void resetDerivatives(float airspeed); /* Accessors */ - bool getEnabled() {return _mTecsEnabled.get() > 0;} - float getThrottleSetpoint() {return _throttleSp;} - float getPitchSetpoint() {return _pitchSp;} + bool getEnabled() { return _mTecsEnabled.get() > 0; } + float getThrottleSetpoint() { return _throttleSp; } + float getPitchSetpoint() { return _pitchSp; } + float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ @@ -109,7 +110,8 @@ protected: BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ /* Other calculation Blocks */ - control::BlockDerivative _airspeedDerivative; + control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ /* Output setpoints */ float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 6165611b9..c95bf1dc9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); */ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); +/** + * Lowpass (cutoff freq.) for airspeed + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); /** * P gain for the airspeed control diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c19579f0f..0813bf7b0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1509,6 +1509,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a874351b3..c42ff0afe 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -355,6 +355,7 @@ struct log_TECS_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -430,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), 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(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index fc530b295..05310e906 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -70,6 +70,7 @@ struct tecs_status_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; -- cgit v1.2.3 From 6d8dfd78f11fdff038f7f7504c507d25429db195 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Jun 2014 15:54:24 +0200 Subject: sdlog2/mtecs: fix length of field name --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c42ff0afe..5645ebcf1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -431,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), 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, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ -- cgit v1.2.3 From 263a2fa9b2258c02bdb0df6746bc289338d63cca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Jun 2014 14:52:09 +0200 Subject: mtecs: add flightpathangle filter --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 19 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 34 +++++++++++++--------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 +++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 +- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 43 insertions(+), 22 deletions(-) (limited to 'src/modules/sdlog2/sdlog2_messages.h') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 48c9079a4..fc0a2551c 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _flightPathAngleLowpass(this, "FPA_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); - /* Filter arispeed */ + /* Filter airspeed */ float airspeedFiltered = _airspeedLowpass.update(airspeed); /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ @@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng } /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; _status.airspeedFiltered = airspeedFiltered; @@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* update parameters first */ updateParams(); + /* Filter flightpathangle */ + float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); + /* calculate values (energies) */ - float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); @@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; - float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; - float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; @@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; } else if (mode == TECS_MODE_LAND) { // only limit pitch but do not limit throttle @@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.flightPathAngleFiltered = flightPathAngleFiltered; _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; _status.totalEnergyRateSp = totalEnergyRateSp; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index c102f5dee..efa89a5d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,12 +104,17 @@ protected: uORB::Publication _status; /**< publish internal values for logging */ /* control blocks */ - BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ - BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output + is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: + output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight + path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration + setpoint */ /* Other calculation Blocks */ + control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ @@ -118,21 +123,22 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ - BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in + last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ - hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; - bool _debug; ///< Set true to enable debug output + bool _debug; ///< Set true to enable debug output static void debug_print(const char *fmt, va_list args); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index bbbb8f9db..5b9238780 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); +/** + * Lowpass (cutoff freq.) for the flight path angle + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); + /** * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0813bf7b0..ab9c1639c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5645ebcf1..a631317df 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -353,6 +353,7 @@ struct log_TECS_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; @@ -431,7 +432,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), 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, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 05310e906..c4d0c1874 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -68,6 +68,7 @@ struct tecs_status_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; -- cgit v1.2.3