Merge branch 'tobi'

2 files changed, 49 insertions, 22 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index c569a6aa4..78fac5c22 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -296,7 +296,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	int calibration_interval_ms = 30 * 1000;
 	unsigned int calibration_counter = 0;
 
-	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
+	float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
 	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
 
 	int fd = open(MAG_DEVICE_PATH, 0);
@@ -353,6 +353,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 	}
 
+	usleep(200000);
+
 	mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
 
 	/* disable calibration mode */
@@ -377,7 +379,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
 	mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
 
-	printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]);
+	if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2]))
+	{
+		mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN");
+	}
+
+	//char buf[52];
+
+	//sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]);
+	//mavlink_log_info(mavlink_fd, buf);
 
 	/* announce and set new offset */
 
@@ -412,7 +422,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		warn("WARNING: auto-save of params to EEPROM failed");
 	}
 
-	mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished");
+	mavlink_log_info(mavlink_fd, "[commander] mag cal finished");
 
 	close(sub_sensor_combined);
 }
@@ -519,7 +529,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	status->flag_preflight_accel_calibration = true;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
-	const int calibration_count = 5000;
+	const int calibration_count = 2500;
 
 	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s raw;
@@ -954,7 +964,7 @@ int commander_main(int argc, char *argv[])
 		deamon_task = task_spawn("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
-					 4096,
+					 8000,
 					 commander_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 22b8d82ee..c02cf2fcf 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -91,6 +91,11 @@ static void usage(const char *reason);
 static int file_exist(const char *filename);
 
 /**
+ * Print the current status.
+ */
+static void print_sdlog_status();
+
+/**
  * Create folder for current logging session.
  */
 static int create_logfolder(char* folder_path);
@@ -103,6 +108,14 @@ usage(const char *reason)
 	errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
 }
 
+// XXX turn this into a C++ class
+unsigned sensor_combined_bytes = 0;
+unsigned actuator_outputs_bytes = 0;
+unsigned actuator_controls_bytes = 0;
+unsigned sysvector_bytes = 0;
+unsigned blackbox_file_bytes = 0;
+uint64_t starttime = 0;
+
 /**
  * The sd log deamon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -145,7 +158,7 @@ int sdlog_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tsdlog is running\n");
+			print_sdlog_status();
 		} else {
 			printf("\tsdlog not started\n");
 		}
@@ -194,7 +207,7 @@ int create_logfolder(char* folder_path) {
 
 int sdlog_thread_main(int argc, char *argv[]) {
 
-	printf("[sdlog] starting\n");
+	warnx("starting\n");
 
 	if (file_exist(mountpoint) != OK) {
 		errx(1, "logging mount point %s not present, exiting.", mountpoint);
@@ -206,21 +219,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* create sensorfile */
 	int sensorfile = -1;
-	unsigned sensor_combined_bytes = 0;
 	int actuator_outputs_file = -1;
-	unsigned actuator_outputs_bytes = 0;
 	int actuator_controls_file = -1;
-	unsigned actuator_controls_bytes = 0;
 	int sysvector_file = -1;
-	unsigned sysvector_bytes = 0;
 	FILE *gpsfile;
-	unsigned blackbox_file_bytes = 0;
 	FILE *blackbox_file;
 	// FILE *vehiclefile;
 
 	char path_buf[64] = ""; // string to hold the path to the sensorfile
 
-	printf("[sdlog] logging to directory %s\n", folder_path);
+	warnx("logging to directory %s\n", folder_path);
 
 	/* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */
 	sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined");
@@ -347,14 +355,14 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* --- GLOBAL POSITION --- */
 	/* subscribe to ORB for global position */
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	fds[fdsc_count].fd = subs.global_pos_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- GPS POSITION --- */
 	/* subscribe to ORB for global position */
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	fds[fdsc_count].fd = subs.gps_pos_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -378,7 +386,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	int poll_count = 0;
 
-	uint64_t starttime = hrt_absolute_time();
+	starttime = hrt_absolute_time();
 
 	while (!thread_should_exit) {
 
@@ -484,6 +492,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			float vbat;
 			float adc[3];
 			float local_pos[3];
+			int32_t gps_pos[3];
 		} sysvector = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -497,7 +506,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
 			.vbat = buf.raw.battery_voltage_v,
 			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
-			.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}
+			.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
+			.gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}
 		};
 		#pragma pack(pop)
 
@@ -506,13 +516,11 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		usleep(10000);
 	}
 
-	unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
-	float mebibytes = bytes / 1024.0f / 1024.0f;
-	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
+	fsync(sysvector_file);
 
-	printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
+	print_sdlog_status();
 
-	printf("[sdlog] exiting.\n");
+	warnx("exiting.\n");
 
 	close(sensorfile);
 	close(actuator_outputs_file);
@@ -525,6 +533,15 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	return 0;
 }
 
+void print_sdlog_status()
+{
+	unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
+	float mebibytes = bytes / 1024.0f / 1024.0f;
+	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
+
+	warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
+}
+
 /**
  * @return 0 if file exists
  */