diff options
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 78 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 62 |
2 files changed, 116 insertions, 24 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4ff6c30aa..6ff56d223 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -85,8 +85,10 @@ #include <uORB/topics/esc_status.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/estimator_status.h> +#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> @@ -942,9 +944,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; struct satellite_info_s sat_info; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -976,13 +980,16 @@ 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; struct log_GS0B_s log_GS0B; 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) @@ -1015,8 +1022,10 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + 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)); @@ -1043,8 +1052,12 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + 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; @@ -1114,7 +1127,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -1127,8 +1140,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.eph = buf_gps_pos.eph; + log_msg.body.log_GPS.epv = buf_gps_pos.epv; log_msg.body.log_GPS.lat = buf_gps_pos.lat; log_msg.body.log_GPS.lon = buf_gps_pos.lon; log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; @@ -1352,7 +1365,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; @@ -1489,15 +1502,52 @@ 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 --- */ + if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + log_msg.msg_type = LOG_TECS_MSG; + log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; + 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; + 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; + 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); + } + + /* --- 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 */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index f4d88f079..8c05e87c5 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 @@ -346,6 +338,53 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; +/* --- TECS - TECS STATUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float flightPathAngleFiltered; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + 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; +}; + +/* --- 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 --- */ @@ -394,13 +433,16 @@ 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, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), + 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"), 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, "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 */ /* FMT: don't write format of format message, it's useless */ |