Merge branch 'master' of github.com:PX4/Firmware into ardrone

2 files changed, 73 insertions, 7 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index dd7b1847b..8d77e7502 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -49,6 +49,7 @@
 #include <debug.h>
 #include <termios.h>
 #include <time.h>
+#include <systemlib/err.h>
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
 #include <uORB/uORB.h>
@@ -73,7 +74,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]);
 /**
  * Open the UART connected to the motor controllers
  */
-static int ardrone_open_uart(struct termios *uart_config_original);
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original);
 
 /**
  * Print the correct usage.
@@ -133,15 +134,15 @@ int ardrone_interface_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int ardrone_open_uart(struct termios *uart_config_original)
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original)
 {
 	/* baud rate */
 	int speed = B115200;
 	int uart;
-	const char* uart_name = "/dev/ttyS1";
+
 
 	/* open uart */
-	printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200\n");
+	printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -182,6 +183,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 {
 	thread_running = true;
 
+	char *device = "/dev/ttyS1";
+
 	/* welcome user */
 	printf("[ardrone_interface] Control started, taking over motors\n");
 
@@ -205,9 +208,20 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 				if (motor > 0 && motor < 5) {
 					test_motor = motor;
 				} else {
+					thread_running = false;
 					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
 				}
 			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
 				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
 			}
 		}
@@ -240,7 +254,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	fflush(stdout);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(&uart_config_original);
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
 	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
 	gpios = ar_multiplexing_init();
@@ -270,7 +284,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	//ar_multiplexing_deinit(gpios);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(&uart_config_original);
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
 	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
 	gpios = ar_multiplexing_init();
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 578bcd875..4be004ad6 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -72,7 +72,7 @@
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
 #include <mavlink/mavlink_log.h>
- 
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -141,6 +141,7 @@ static void led_deinit(void);
 static int led_toggle(int led);
 static int led_on(int led);
 static int led_off(int led);
+static int pm_save_eeprom(bool only_unsaved);
 static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
@@ -271,6 +272,33 @@ static void cal_bsort(float a[], int n)
 	}
 }
 
+static const char *parameter_file = "/eeprom/parameters";
+
+static int pm_save_eeprom(bool only_unsaved)
+{
+	/* delete the file in case it exists */
+	unlink(parameter_file);
+
+	/* create the file */
+	int fd = open(parameter_file, O_WRONLY | O_CREAT | O_EXCL);
+
+	if (fd < 0) {
+		warn("opening '%s' for writing failed", parameter_file);
+		return -1;
+	}
+
+	int result = param_export(fd, only_unsaved);
+	close(fd);
+
+	if (result != 0) {
+		unlink(parameter_file);
+		warn("error exporting parameters to '%s'", parameter_file);
+		return -2;
+	}
+
+	return 0;
+}
+
 void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	/* set to mag calibration mode */
@@ -492,6 +520,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	free(mag_minima[1]);
 	free(mag_minima[2]);
 
+	/* auto-save to EEPROM */
+	int save_ret = pm_save_eeprom(false);
+	if(save_ret != 0) {
+		warn("WARNING: auto-save of params to EEPROM failed");
+	}
+
+	mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished");
+
 	close(sub_sensor_combined);
 }
 
@@ -571,10 +607,18 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 		warn("WARNING: failed to set scale / offsets for gyro");
 	close(fd);
 
+	/* auto-save to EEPROM */
+	int save_ret = pm_save_eeprom(false);
+	if(save_ret != 0) {
+		warn("WARNING: auto-save of params to EEPROM failed");
+	}
+
 	/* exit to gyro calibration mode */
 	status->flag_preflight_gyro_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
+	mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished");
+
 	// char offset_output[50];
 	// sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
 	// mavlink_log_info(mavlink_fd, offset_output);
@@ -680,10 +724,18 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 		warn("WARNING: failed to set scale / offsets for accel");
 	close(fd);
 
+	/* auto-save to EEPROM */
+	int save_ret = pm_save_eeprom(false);
+	if(save_ret != 0) {
+		warn("WARNING: auto-save of params to EEPROM failed");
+	}
+
 	/* exit to gyro calibration mode */
 	status->flag_preflight_accel_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
+	mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished");
+
 	// char offset_output[50];
 	// sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
 	// 	(double)accel_offset[1], (double)accel_offset[2]);