Merge branch 'master' into navigator_rewrite

Conflicts: src/drivers/gps/gps.cpp src/drivers/gps/mtk.cpp src/modules/commander/commander.cpp src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp src/modules/navigator/mission.cpp src/modules/navigator/mission.h src/modules/navigator/navigator_main.cpp src/modules/navigator/navigator_state.h src/modules/position_estimator_inav/position_estimator_inav_main.c
2026-10-06 13:28:53 +08:00 · 2014-05-26 20:19:11 +02:00
parent 68352cb923 36495cdb62
commit 063caba36b
173 changed files with 5312 additions and 1890 deletions
@@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"

 SRCS = sdlog2.c \
       logbuffer.c
+
+MODULE_STACKSIZE = 1200
@@ -89,6 +89,7 @@

 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
 #include <version/version.h>

 #include <mavlink/mavlink_log.h>
@@ -97,6 +98,36 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"

+/**
+ * Logging rate.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 sets the minimum rate,
+ * any other value is interpreted as rate in Hertz. This
+ * parameter is only read out before logging starts (which
+ * commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_RATE, -1);
+
+/**
+ * Enable extended logging mode.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 disables extended
+ * logging mode, a value of 1 enables it. This
+ * parameter is only read out before logging starts
+ * (which commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_EXT, -1);
+
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
@@ -112,12 +143,14 @@ static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
 static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
-static const int MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
+static const unsigned MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
+static const unsigned MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 static const int MAX_WRITE_CHUNK = 512;
 static const int MIN_BYTES_TO_WRITE = 512;

+static bool _extended_logging = false;
+
 static const char *log_root = "/fs/microsd/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
@@ -218,6 +251,8 @@ static int create_log_dir(void);
 */
 static int open_log_file(void);

+static int open_perf_file(const char* str);
+
 static void
 sdlog2_usage(const char *reason)
 {
@@ -225,12 +260,13 @@ sdlog2_usage(const char *reason)
 		fprintf(stderr, "%s\n", reason);
 	}

-	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
+	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 8\n"
 		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
 		 "\t-a\tLog only when armed (can be still overriden by command)\n"
-		 "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-t\tUse date/time for naming log directories and files\n"
+		 "\t-x\tExtended logging");
 }

 /**
@@ -349,8 +385,8 @@ int create_log_dir()
 int open_log_file()
 {
 	/* string to hold the path to the log */
-	char log_file_name[16] = "";
-	char log_file_path[48] = "";
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";

 	if (log_name_timestamp && gps_time != 0) {
 		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
@@ -378,7 +414,7 @@ int open_log_file()

 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			warnx("all %d possible files exist already", MAX_NO_LOGFILE);
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -387,7 +423,58 @@ int open_log_file()

 	if (fd < 0) {
 		warn("failed opening log: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+
+	} else {
+		warnx("log file: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+	}
+
+	return fd;
+}
+
+int open_perf_file(const char* str)
+{
+	/* string to hold the path to the log */
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+	} else {
+		unsigned file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+			if (!file_exist(log_file_path)) {
+				break;
+			}
+
+			file_number++;
+		}
+
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			return -1;
+		}
+	}
+
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+
+	if (fd < 0) {
+		warn("failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);

 	} else {
 		warnx("log file: %s", log_file_name);
@@ -529,6 +616,12 @@ void sdlog2_start_log()
 		errx(1, "error creating logwriter thread");
 	}

+	/* write all performance counters */
+	int perf_fd = open_perf_file("preflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	logging_enabled = true;
 }

@@ -556,6 +649,12 @@ void sdlog2_stop_log()
 	logwriter_pthread = 0;
 	pthread_attr_destroy(&logwriter_attr);

+	/* write all performance counters */
+	int perf_fd = open_perf_file("postflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	sdlog2_status();
 }

@@ -572,7 +671,7 @@ int write_formats(int fd)
 	int written = 0;

 	/* fill message format packet for each format and write it */
-	for (int i = 0; i < log_formats_num; i++) {
+	for (unsigned i = 0; i < log_formats_num; i++) {
 		log_msg_format.body = log_formats[i];
 		written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
@@ -679,12 +778,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;

-	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
+	while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);

-				if (r <= 0) {
+				if (r == 0) {
 					r = 1;
 				}

@@ -715,6 +814,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_name_timestamp = true;
 			break;

+		case 'x':
+			_extended_logging = true;
+			break;
+
 		case '?':
 			if (optopt == 'c') {
 				warnx("option -%c requires an argument", optopt);
@@ -741,6 +844,44 @@ int sdlog2_thread_main(int argc, char *argv[])

 	gps_time = 0;

+	/* interpret logging params */
+
+	param_t log_rate_ph = param_find("SDLOG_RATE");
+
+	if (log_rate_ph != PARAM_INVALID) {
+		int32_t param_log_rate;
+		param_get(log_rate_ph, &param_log_rate);
+
+		if (param_log_rate > 0) {
+
+			/* we can't do more than ~ 500 Hz, even with a massive buffer */
+			if (param_log_rate > 500) {
+				param_log_rate = 500;
+			}
+
+			sleep_delay = 1000000 / param_log_rate;
+		} else if (param_log_rate == 0) {
+			/* we need at minimum 10 Hz to be able to see anything */
+			sleep_delay = 1000000 / 10;
+		}
+	}
+
+	param_t log_ext_ph = param_find("SDLOG_EXT");
+
+	if (log_ext_ph != PARAM_INVALID) {
+
+		int32_t param_log_extended;
+		param_get(log_ext_ph, &param_log_extended);
+
+		if (param_log_extended > 0) {
+			_extended_logging = true;
+		} else if (param_log_extended == 0) {
+			_extended_logging = false;
+		}
+		/* any other value means to ignore the parameter, so no else case */
+
+	}
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);

@@ -834,6 +975,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ESTM_s log_ESTM;
 			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;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -967,8 +1112,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}

-		/* --- GPS POSITION --- */
+		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
+
+			float snr_mean = 0.0f;
+
+			for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
+				snr_mean += buf_gps_pos.satellite_snr[i];
+			}
+
+			snr_mean /= buf_gps_pos.satellites_visible;
+
 			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;
@@ -981,7 +1135,48 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
 			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
 			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+			log_msg.body.log_GPS.snr_mean = snr_mean;
+			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
+			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
+
+			if (_extended_logging) {
+				/* log the SNR of each satellite for a detailed view of signal quality */
+				unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
+
+				log_msg.msg_type = LOG_GS0A_MSG;
+				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
+				/* fill set A */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					int satindex = buf_gps_pos.satellite_prn[i] - 1;
+
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+
+				log_msg.msg_type = LOG_GS0B_MSG;
+				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+				/* fill set B */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					/* get second bank of satellites, thus deduct bank size from index */
+					int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0B);
+			}
 		}

 		/* --- SENSOR COMBINED --- */
@@ -1105,10 +1300,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
 			log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
 			log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
-			log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
-			log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+			log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) |
+											  (buf.local_pos.z_valid ? 2 : 0) |
+											  (buf.local_pos.v_xy_valid ? 4 : 0) |
+											  (buf.local_pos.v_z_valid ? 8 : 0) |
+											  (buf.local_pos.xy_global ? 16 : 0) |
+											  (buf.local_pos.z_global ? 32 : 0);
 			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+			log_msg.body.log_LPOS.eph = buf.local_pos.eph;
+			log_msg.body.log_LPOS.epv = buf.local_pos.epv;
 			LOGBUFFER_WRITE_AND_COUNT(LPOS);
 		}

@@ -1320,6 +1521,7 @@ void sdlog2_status()
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;

 	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
 	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
 }

@@ -109,10 +109,11 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
-	uint8_t xy_flags;
-	uint8_t z_flags;
+	uint8_t pos_flags;
 	uint8_t landed;
 	uint8_t ground_dist_flags;
+	float eph;
+	float epv;
 };

 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -138,6 +139,10 @@ struct log_GPS_s {
 	float vel_e;
 	float vel_d;
 	float cog;
+	uint8_t sats;
+	uint16_t snr_mean;
+	uint16_t noise_per_ms;
+	uint16_t jamming_indicator;
 };

 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
@@ -317,6 +322,30 @@ struct log_VICN_s {
 	float yaw;
 };

+/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
+#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 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 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 29
+struct log_GS1B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/

 /* --- TIME - TIME STAMP --- */
@@ -348,9 +377,9 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
-	LOG_FORMAT(LPOS, "ffffffffLLfBBBB",	"X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
-	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
@@ -368,6 +397,10 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
 	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"),

 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */