From 2f4afa2da7ac24ccd38a3f532b749e1e46680e76 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 8 Oct 2015 21:19:51 +0200 Subject: [PATCH] added control state to logging --- src/modules/sdlog2/sdlog2.c | 18 +++++++ src/modules/sdlog2/sdlog2_messages.h | 81 ++++++++++++++++------------ 2 files changed, 65 insertions(+), 34 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 253949b26e..8ee40f1661 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -1108,6 +1109,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; + struct control_state_s ctrl_state; struct vehicle_attitude_setpoint_s att_sp; struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; @@ -1158,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; + struct log_CTS_s log_CTS; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1199,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int vtol_status_sub; int sensor_sub; int att_sub; + int ctrl_state_sub; int att_sp_sub; int rates_sp_sub; int act_outputs_sub; @@ -1236,6 +1240,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.gps_pos_sub = -1; subs.sensor_sub = -1; subs.att_sub = -1; + subs.ctrl_state_sub = -1; subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; @@ -1714,6 +1719,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(FLOW); } + /* --- CONTROL STATE --- */ + if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { + log_msg.msg_type = LOG_CTS_MSG; + log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; + log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; + log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; + log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; + log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; + log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; + log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; + LOGBUFFER_WRITE_AND_COUNT(CTS); + } + /* --- RC CHANNELS --- */ if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7f77a58e7b..16fafd3165 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -185,8 +185,20 @@ struct log_STAT_s { float load; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 11 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; uint8_t channel_count; @@ -194,13 +206,13 @@ struct log_RC_s { }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; @@ -208,7 +220,7 @@ struct log_AIRS_s { }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -216,7 +228,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 15 +#define LOG_FLOW_MSG 16 struct log_FLOW_s { uint8_t sensor_id; float pixel_flow_x_integral; @@ -233,7 +245,7 @@ struct log_FLOW_s { }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ -#define LOG_GPOS_MSG 16 +#define LOG_GPOS_MSG 17 struct log_GPOS_s { int32_t lat; int32_t lon; @@ -247,7 +259,7 @@ struct log_GPOS_s { }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ -#define LOG_GPSP_MSG 17 +#define LOG_GPSP_MSG 18 struct log_GPSP_s { uint8_t nav_state; int32_t lat; @@ -261,7 +273,7 @@ struct log_GPSP_s { }; /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 18 +#define LOG_ESC_MSG 19 struct log_ESC_s { uint16_t counter; uint8_t esc_count; @@ -278,7 +290,7 @@ struct log_ESC_s { }; /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ -#define LOG_GVSP_MSG 19 +#define LOG_GVSP_MSG 20 struct log_GVSP_s { float vx; float vy; @@ -286,7 +298,7 @@ struct log_GVSP_s { }; /* --- BATT - BATTERY --- */ -#define LOG_BATT_MSG 20 +#define LOG_BATT_MSG 21 struct log_BATT_s { float voltage; float voltage_filtered; @@ -295,7 +307,7 @@ struct log_BATT_s { }; /* --- DIST - RANGE SENSOR DISTANCE --- */ -#define LOG_DIST_MSG 21 +#define LOG_DIST_MSG 22 struct log_DIST_s { uint8_t id; uint8_t type; @@ -304,11 +316,11 @@ struct log_DIST_s { float covariance; }; -/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ +/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */ /* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 24 +#define LOG_PWR_MSG 25 struct log_PWR_s { float peripherals_5v; float servo_rail_5v; @@ -321,7 +333,7 @@ struct log_PWR_s { }; /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ -#define LOG_MOCP_MSG 25 +#define LOG_MOCP_MSG 26 struct log_MOCP_s { float qw; float qx; @@ -333,31 +345,31 @@ struct log_MOCP_s { }; /* --- GS0A - GPS SNR #0, SAT GROUP A --- */ -#define LOG_GS0A_MSG 26 +#define LOG_GS0A_MSG 27 struct log_GS0A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS0B - GPS SNR #0, SAT GROUP B --- */ -#define LOG_GS0B_MSG 27 +#define LOG_GS0B_MSG 28 struct log_GS0B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1A - GPS SNR #1, SAT GROUP A --- */ -#define LOG_GS1A_MSG 28 +#define LOG_GS1A_MSG 29 struct log_GS1A_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- GS1B - GPS SNR #1, SAT GROUP B --- */ -#define LOG_GS1B_MSG 29 +#define LOG_GS1B_MSG 30 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 +#define LOG_TECS_MSG 31 struct log_TECS_s { float altitudeSp; float altitudeFiltered; @@ -378,7 +390,7 @@ struct log_TECS_s { }; /* --- WIND - WIND ESTIMATE --- */ -#define LOG_WIND_MSG 31 +#define LOG_WIND_MSG 32 struct log_WIND_s { float x; float y; @@ -387,7 +399,7 @@ struct log_WIND_s { }; /* --- EST0 - ESTIMATOR STATUS --- */ -#define LOG_EST0_MSG 32 +#define LOG_EST0_MSG 33 struct log_EST0_s { float s[12]; uint8_t n_states; @@ -397,28 +409,28 @@ struct log_EST0_s { }; /* --- EST1 - ESTIMATOR STATUS --- */ -#define LOG_EST1_MSG 33 +#define LOG_EST1_MSG 34 struct log_EST1_s { float s[16]; }; /* --- EST2 - ESTIMATOR STATUS --- */ -#define LOG_EST2_MSG 34 +#define LOG_EST2_MSG 35 struct log_EST2_s { float cov[12]; }; /* --- EST3 - ESTIMATOR STATUS --- */ -#define LOG_EST3_MSG 35 +#define LOG_EST3_MSG 36 struct log_EST3_s { float cov[16]; }; /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 36 -#define LOG_TEL1_MSG 37 -#define LOG_TEL2_MSG 38 -#define LOG_TEL3_MSG 39 +#define LOG_TEL0_MSG 37 +#define LOG_TEL1_MSG 38 +#define LOG_TEL2_MSG 39 +#define LOG_TEL3_MSG 40 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -431,7 +443,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 40 +#define LOG_VISN_MSG 41 struct log_VISN_s { float x; float y; @@ -446,7 +458,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 41 +#define LOG_ENCD_MSG 42 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -455,22 +467,22 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 42 +#define LOG_AIR1_MSG 43 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 43 +#define LOG_VTOL_MSG 44 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 44 +#define LOG_TSYN_MSG 45 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 45 +#define LOG_MACS_MSG 47 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; @@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), + LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),