Merge branch 'master' into mpc_yaw_fix

2 files changed, 52 insertions, 9 deletions

diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index d11a67fc6..cec2fdc43 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -99,6 +99,8 @@ struct listener {
 	uintptr_t	arg;
 };
 
+uint16_t cm_uint16_from_m_float(float m);
+
 static void	l_sensor_combined(const struct listener *l);
 static void	l_vehicle_attitude(const struct listener *l);
 static void	l_vehicle_gps_position(const struct listener *l);
@@ -150,6 +152,19 @@ static const struct listener listeners[] = {
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
 
+uint16_t
+cm_uint16_from_m_float(float m)
+{
+	if (m < 0.0f) {
+		return 0;
+
+	} else if (m > 655.35f) {
+		return 65535;
+	}
+
+	return (uint16_t)(m * 100.0f);
+}
+
 void
 l_sensor_combined(const struct listener *l)
 {
@@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l)
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
 	float cog_deg = gps.cog_rad;
+
 	if (cog_deg > M_PI_F)
 		cog_deg -= 2.0f * M_PI_F;
+
 	cog_deg *= M_RAD_TO_DEG_F;
 
 
@@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     gps.eph_m * 1e2f, // from m to cm
-				     gps.epv_m * 1e2f, // from m to cm
+				     cm_uint16_from_m_float(gps.eph_m),
+				     cm_uint16_from_m_float(gps.epv_m),
 				     gps.vel_m_s * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from rad to deg * 100
+				     cog_deg * 1e2f, // from deg to deg * 100
 				     gps.satellites_visible);
 
 	/* update SAT info every 10 seconds */
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 59cbcf5fb..ccdb2ea38 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -509,31 +509,57 @@ int
 param_save_default(void)
 {
 	int result;
+	unsigned retries = 0;
 
 	/* delete the file in case it exists */
 	struct stat buffer;
 	if (stat(param_get_default_file(), &buffer) == 0) {
-		result = unlink(param_get_default_file());
+
+		do {
+			result = unlink(param_get_default_file());
+			if (result != 0) {
+				retries++;
+				usleep(1000 * retries);
+			}
+		} while (result != OK && retries < 10);
+
 		if (result != OK)
 			warnx("unlinking file %s failed.", param_get_default_file());
 	}
 
 	/* create the file */
-	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+	int fd;
 
-	if (fd < 0) {
+	do {
 		/* do another attempt in case the unlink call is not synced yet */
-		usleep(5000);
 		fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+		if (fd < 0) {
+			retries++;
+			usleep(1000 * retries);
+		}
 
+	} while (fd < 0 && retries < 10);
+
+	if (fd < 0) {
+		
 		warn("opening '%s' for writing failed", param_get_default_file());
 		return fd;
 	}
 
-	result = param_export(fd, false);
+	do {
+		result = param_export(fd, false);
+
+		if (result != OK) {
+			retries++;
+			usleep(1000 * retries);
+		}
+
+	} while (result != 0 && retries < 10);
+
+
 	close(fd);
 
-	if (result != 0) {
+	if (result != OK) {
 		warn("error exporting parameters to '%s'", param_get_default_file());
 		(void)unlink(param_get_default_file());
 		return result;