fixed logging

src/modules/sdlog2/sdlog2.c

@@ -1060,7 +1060,6 @@ 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;
@@ -1091,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vtol_vehicle_status_s vtol_status;
 		struct time_offset_s time_offset;
 		struct mc_att_ctrl_status_s mc_att_ctrl_status;
+		struct control_state_s ctrl_state;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1111,7 +1111,6 @@ 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;
@@ -1140,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ENCD_s log_ENCD;
 			struct log_TSYN_s log_TSYN;
 			struct log_MACS_s log_MACS;
+			struct log_CTS_s log_CTS;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1153,7 +1153,6 @@ 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;
@@ -1183,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int encoders_sub;
 		int tsync_sub;
 		int mc_att_ctrl_status_sub;
+		int ctrl_state_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1191,7 +1191,6 @@ 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;
@@ -1217,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.wind_sub = -1;
 	subs.tsync_sub = -1;
 	subs.mc_att_ctrl_status_sub = -1;
+	subs.ctrl_state_sub = -1;
 	subs.encoders_sub = -1;
 
 	/* add new topics HERE */
@@ -1670,19 +1670,6 @@ 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;
@@ -1874,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(MACS);
 		}
 
+		/* --- 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);
+		}
+
 		/* 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 */

src/modules/sdlog2/sdlog2_messages.h

@@ -185,20 +185,8 @@ 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 12
+#define LOG_RC_MSG 11
 struct log_RC_s {
 	float channel[8];
 	uint8_t channel_count;
@@ -206,13 +194,13 @@ struct log_RC_s {
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
-#define LOG_OUT0_MSG 13
+#define LOG_OUT0_MSG 12
 struct log_OUT0_s {
 	float output[8];
 };
 
 /* --- AIRS - AIRSPEED --- */
-#define LOG_AIRS_MSG 14
+#define LOG_AIRS_MSG 13
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
@@ -220,7 +208,7 @@ struct log_AIRS_s {
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
-#define LOG_ARSP_MSG 15
+#define LOG_ARSP_MSG 14
 struct log_ARSP_s {
 	float roll_rate_sp;
 	float pitch_rate_sp;
@@ -228,7 +216,7 @@ struct log_ARSP_s {
 };
 
 /* --- FLOW - OPTICAL FLOW --- */
-#define LOG_FLOW_MSG 16
+#define LOG_FLOW_MSG 15
 struct log_FLOW_s {
 	uint8_t sensor_id;
 	float pixel_flow_x_integral;
@@ -245,7 +233,7 @@ struct log_FLOW_s {
 };
 
 /* --- GPOS - GLOBAL POSITION ESTIMATE --- */
-#define LOG_GPOS_MSG 17
+#define LOG_GPOS_MSG 16
 struct log_GPOS_s {
 	int32_t lat;
 	int32_t lon;
@@ -259,7 +247,7 @@ struct log_GPOS_s {
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
-#define LOG_GPSP_MSG 18
+#define LOG_GPSP_MSG 17
 struct log_GPSP_s {
 	uint8_t nav_state;
 	int32_t lat;
@@ -273,7 +261,7 @@ struct log_GPSP_s {
 };
 
 /* --- ESC - ESC STATE --- */
-#define LOG_ESC_MSG 19
+#define LOG_ESC_MSG 18
 struct log_ESC_s {
 	uint16_t counter;
 	uint8_t  esc_count;
@@ -290,7 +278,7 @@ struct log_ESC_s {
 };
 
 /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
-#define LOG_GVSP_MSG 20
+#define LOG_GVSP_MSG 19
 struct log_GVSP_s {
 	float vx;
 	float vy;
@@ -298,7 +286,7 @@ struct log_GVSP_s {
 };
 
 /* --- BATT - BATTERY --- */
-#define LOG_BATT_MSG 21
+#define LOG_BATT_MSG 20
 struct log_BATT_s {
 	float voltage;
 	float voltage_filtered;
@@ -307,7 +295,7 @@ struct log_BATT_s {
 };
 
 /* --- DIST - RANGE SENSOR DISTANCE --- */
-#define LOG_DIST_MSG 22
+#define LOG_DIST_MSG 21
 struct log_DIST_s {
 	uint8_t id;
 	uint8_t type;
@@ -316,11 +304,11 @@ struct log_DIST_s {
 	float covariance;
 };
 
-/* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */
+/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
 
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
-#define LOG_PWR_MSG 25
+#define LOG_PWR_MSG 24
 struct log_PWR_s {
 	float peripherals_5v;
 	float servo_rail_5v;
@@ -333,7 +321,7 @@ struct log_PWR_s {
 };
 
 /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
-#define LOG_MOCP_MSG 26
+#define LOG_MOCP_MSG 25
 struct log_MOCP_s {
 	float qw;
 	float qx;
@@ -345,31 +333,31 @@ struct log_MOCP_s {
 };
 
 /* --- GS0A - GPS SNR #0, SAT GROUP A --- */
-#define LOG_GS0A_MSG 27
+#define LOG_GS0A_MSG 26
 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 28
+#define LOG_GS0B_MSG 27
 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 29
+#define LOG_GS1A_MSG 28
 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 30
+#define LOG_GS1B_MSG 29
 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 31
+#define LOG_TECS_MSG 30
 struct log_TECS_s {
 	float altitudeSp;
 	float altitudeFiltered;
@@ -390,7 +378,7 @@ struct log_TECS_s {
 };
 
 /* --- WIND - WIND ESTIMATE --- */
-#define LOG_WIND_MSG 32
+#define LOG_WIND_MSG 31
 struct log_WIND_s {
 	float x;
 	float y;
@@ -399,7 +387,7 @@ struct log_WIND_s {
 };
 
 /* --- EST0 - ESTIMATOR STATUS --- */
-#define LOG_EST0_MSG 33
+#define LOG_EST0_MSG 32
 struct log_EST0_s {
 	float s[12];
 	uint8_t n_states;
@@ -409,28 +397,28 @@ struct log_EST0_s {
 };
 
 /* --- EST1 - ESTIMATOR STATUS --- */
-#define LOG_EST1_MSG 34
+#define LOG_EST1_MSG 33
 struct log_EST1_s {
 	float s[16];
 };
 
 /* --- EST2 - ESTIMATOR STATUS --- */
-#define LOG_EST2_MSG 35
+#define LOG_EST2_MSG 34
 struct log_EST2_s {
     float cov[12];
 };
 
 /* --- EST3 - ESTIMATOR STATUS --- */
-#define LOG_EST3_MSG 36
+#define LOG_EST3_MSG 35
 struct log_EST3_s {
     float cov[16];
 };
 
 /* --- TEL0..3 - TELEMETRY STATUS --- */
-#define LOG_TEL0_MSG 37
-#define LOG_TEL1_MSG 38
-#define LOG_TEL2_MSG 39
-#define LOG_TEL3_MSG 40
+#define LOG_TEL0_MSG 36
+#define LOG_TEL1_MSG 37
+#define LOG_TEL2_MSG 38
+#define LOG_TEL3_MSG 39
 struct log_TEL_s {
 	uint8_t rssi;
 	uint8_t remote_rssi;
@@ -443,7 +431,7 @@ struct log_TEL_s {
 };
 
 /* --- VISN - VISION POSITION --- */
-#define LOG_VISN_MSG 41
+#define LOG_VISN_MSG 40
 struct log_VISN_s {
 	float x;
 	float y;
@@ -458,7 +446,7 @@ struct log_VISN_s {
 };
 
 /* --- ENCODERS - ENCODER DATA --- */
-#define LOG_ENCD_MSG 42
+#define LOG_ENCD_MSG 41
 struct log_ENCD_s {
 	int64_t cnt0;
 	float vel0;
@@ -467,28 +455,40 @@ struct log_ENCD_s {
 };
 
 /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
-#define LOG_AIR1_MSG 43
+#define LOG_AIR1_MSG 42
 
 /* --- VTOL - VTOL VEHICLE STATUS */
-#define LOG_VTOL_MSG 44
+#define LOG_VTOL_MSG 43
 struct log_VTOL_s {
 	float airspeed_tot;
 };
 
 /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
-#define LOG_TSYN_MSG 45
+#define LOG_TSYN_MSG 44
 struct log_TSYN_s {
 	uint64_t time_offset;
 };
 
 /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
-#define LOG_MACS_MSG 47
+#define LOG_MACS_MSG 45
 struct log_MACS_s {
 	float roll_rate_integ;
 	float pitch_rate_integ;
 	float yaw_rate_integ;
 };
 
+/* --- CONTROL STATE --- */
+#define LOG_CTS_MSG 47
+struct log_CTS_s {
+	float vx_body;
+	float vy_body;
+	float vz_body;
+	float airspeed;
+	float roll_rate;
+	float pitch_rate;
+	float yaw_rate;
+};
+
 /* WARNING: ID 46 is already in use for ATTC1 */
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/