auto save after calibration (however the rest is reset to stock)

1 files changed, 47 insertions, 1 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index c8564792f..18dd0ceb6 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -71,7 +71,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>
@@ -136,6 +136,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);
@@ -266,6 +267,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 */
@@ -487,6 +515,12 @@ 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(true);
+	if(save_ret != 0) {
+		warn("WARNING: auto-save of params to EEPROM failed");
+	}
+
 	close(sub_sensor_combined);
 }
 
@@ -566,6 +600,12 @@ 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(true);
+	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);
@@ -675,6 +715,12 @@ 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(true);
+	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);