Merge branch 'master' into mpc_yaw_fix

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -262,7 +262,7 @@ CONFIG_STM32_USART=y
 # U[S]ART Configuration
 #
 # CONFIG_USART1_RS485 is not set
-# CONFIG_USART1_RXDMA is not set
+CONFIG_USART1_RXDMA=y
 # CONFIG_USART2_RS485 is not set
 CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RXDMA is not set

src/drivers/gps/gps.cpp

@@ -85,7 +85,7 @@ static const int ERROR = -1;
 class GPS : public device::CDev
 {
 public:
-	GPS(const char* uart_path);
+	GPS(const char *uart_path);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -156,7 +156,7 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char* uart_path) :
+GPS::GPS(const char *uart_path) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
@@ -192,6 +192,7 @@ GPS::~GPS()
 	/* well, kill it anyway, though this will probably crash */
 	if (_task != -1)
 		task_delete(_task);
+
 	g_dev = nullptr;
 
 }
@@ -270,19 +271,24 @@ GPS::task_main()
 		}
 
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
+		case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
+			_Helper = new UBX(_serial_fd, &_report);
-				break;
+			break;
-			case GPS_DRIVER_MODE_MTK:
+
-				_Helper = new MTK(_serial_fd, &_report);
+		case GPS_DRIVER_MODE_MTK:
-				break;
+			_Helper = new MTK(_serial_fd, &_report);
-			case GPS_DRIVER_MODE_NMEA:
+			break;
-				//_Helper = new NMEA(); //TODO: add NMEA
+
-				break;
+		case GPS_DRIVER_MODE_NMEA:
-			default:
+			//_Helper = new NMEA(); //TODO: add NMEA
-				break;
+			break;
+
+		default:
+			break;
 		}
+
 		unlock();
+
 		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
 
@@ -294,6 +300,7 @@ GPS::task_main()
 				/* opportunistic publishing - else invalid data would end up on the bus */
 				if (_report_pub > 0) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
 				} else {
 					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
 				}
@@ -310,10 +317,30 @@ GPS::task_main()
 				}
 
 				if (!_healthy) {
-					warnx("module found");
+					char *mode_str = "unknown";
+
+					switch (_mode) {
+					case GPS_DRIVER_MODE_UBX:
+						mode_str = "UBX";
+						break;
+
+					case GPS_DRIVER_MODE_MTK:
+						mode_str = "MTK";
+						break;
+
+					case GPS_DRIVER_MODE_NMEA:
+						mode_str = "NMEA";
+						break;
+
+					default:
+						break;
+					}
+
+					warnx("module found: %s", mode_str);
 					_healthy = true;
 				}
 			}
+
 			if (_healthy) {
 				warnx("module lost");
 				_healthy = false;
@@ -322,25 +349,29 @@ GPS::task_main()
 
 			lock();
 		}
+
 		lock();
 
 		/* select next mode */
 		switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
+		case GPS_DRIVER_MODE_UBX:
-				_mode = GPS_DRIVER_MODE_MTK;
+			_mode = GPS_DRIVER_MODE_MTK;
-				break;
+			break;
-			case GPS_DRIVER_MODE_MTK:
+
-				_mode = GPS_DRIVER_MODE_UBX;
+		case GPS_DRIVER_MODE_MTK:
-				break;
+			_mode = GPS_DRIVER_MODE_UBX;
-		//				case GPS_DRIVER_MODE_NMEA:
+			break;
-		//					_mode = GPS_DRIVER_MODE_UBX;
+
-		//					break;
+			//				case GPS_DRIVER_MODE_NMEA:
-			default:
+			//					_mode = GPS_DRIVER_MODE_UBX;
-				break;
+			//					break;
+		default:
+			break;
 		}
 
 	}
-	debug("exiting");
+
+	warnx("exiting");
 
 	::close(_serial_fd);
 
@@ -361,23 +392,29 @@ void
 GPS::print_info()
 {
 	switch (_mode) {
-		case GPS_DRIVER_MODE_UBX:
+	case GPS_DRIVER_MODE_UBX:
-			warnx("protocol: UBX");
+		warnx("protocol: UBX");
-			break;
+		break;
-		case GPS_DRIVER_MODE_MTK:
+
-			warnx("protocol: MTK");
+	case GPS_DRIVER_MODE_MTK:
-			break;
+		warnx("protocol: MTK");
-		case GPS_DRIVER_MODE_NMEA:
+		break;
-			warnx("protocol: NMEA");
+
-			break;
+	case GPS_DRIVER_MODE_NMEA:
-		default:
+		warnx("protocol: NMEA");
-			break;
+		break;
+
+	default:
+		break;
 	}
+
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
+		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
+				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -428,6 +465,7 @@ start(const char *uart_path)
 		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
 		goto fail;
 	}
+
 	exit(0);
 
 fail:
@@ -503,7 +541,7 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char* device_name = GPS_DEFAULT_UART_PORT;
+	char *device_name = GPS_DEFAULT_UART_PORT;
 
 	/*
 	 * Start/load the driver.
@@ -513,15 +551,18 @@ gps_main(int argc, char *argv[])
 		if (argc > 3) {
 			if (!strcmp(argv[2], "-d")) {
 				device_name = argv[3];
+
 			} else {
 				goto out;
 			}
 		}
+
 		gps::start(device_name);
 	}
 
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();
+
 	/*
 	 * Test the driver/device.
 	 */

src/drivers/gps/gps_helper.cpp

@@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
-	warnx("try baudrate: %d\n", speed);
+		warnx("try baudrate: %d\n", speed);
 
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;
 	}
+
 	struct termios uart_config;
+
 	int termios_state;
 
 	/* fill the struct for the new configuration */
@@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
 		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
 		return -1;
 	}
+
 	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERROR setting baudrate (tcsetattr)\n");
 		return -1;
 	}
+
 	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
 	return 0;
 }

src/drivers/gps/gps_helper.h

@@ -33,7 +33,7 @@
  *
  ****************************************************************************/
 
-/** 
+/**
  * @file gps_helper.h
  */
 

src/drivers/gps/mtk.cpp

@@ -48,9 +48,9 @@
 
 
 MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
+	_fd(fd),
-_gps_position(gps_position),
+	_gps_position(gps_position),
-_mtk_revision(0)
+	_mtk_revision(0)
 {
 	decode_init();
 }
@@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
+
 	usleep(10000);
 
 	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
 					handle_message(packet);
 					return 1;
 				}
+
 				/* in case we keep trying but only get crap from GPS */
-				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				if (time_started + timeout * 1000 < hrt_absolute_time()) {
 					return -1;
 				}
+
 				j++;
 			}
+
 			/* everything is read */
 			j = count = 0;
 		}
@@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 		if (b == MTK_SYNC1_V16) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 16;
+
 		} else if (b == MTK_SYNC1_V19) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
 			_mtk_revision = 19;
@@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			add_byte_to_checksum(b);
 
 		// Fill packet buffer
-		((uint8_t*)(&packet))[_rx_count] = b;
+		((uint8_t *)(&packet))[_rx_count] = b;
 		_rx_count++;
 
 		/* Packet size minus checksum, XXX ? */
@@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 			/* Compare checksum */
 			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
 				ret = 1;
+
 			} else {
 				warnx("MTK Checksum invalid");
 				ret = -1;
 			}
+
 			// Reset state machine to decode next packet
 			decode_init();
 		}
 	}
+
 	return ret;
 }
 
@@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	if (_mtk_revision == 16) {
 		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
 		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
 	} else if (_mtk_revision == 19) {
 		_gps_position->lat = packet.latitude; // both degrees*1e7
 		_gps_position->lon = packet.longitude; // both degrees*1e7
+
 	} else {
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
 	}
+
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
 	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
 	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
-	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
 

src/drivers/gps/ubx.cpp

@@ -60,13 +60,14 @@
 #define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
 #define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
 #define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
+#define DISABLE_MSG_INTERVAL	1000000	// us, try to disable message with this interval
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_fd(fd),
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
-	_disable_cmd_counter(0)
+	_disable_cmd_last(0)
 {
 	decode_init();
 }
@@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
+		warnx("ubx: msg rate configuration failed: NAV POSLLH");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
+		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL\n");
+		warnx("ubx: msg rate configuration failed: NAV SOL");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED\n");
+		warnx("ubx: msg rate configuration failed: NAV VELNED");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
+		warnx("ubx: msg rate configuration failed: NAV SVINFO");
 		return 1;
 	}
 
@@ -271,11 +272,18 @@ UBX::receive(unsigned timeout)
 
 		if (ret < 0) {
 			/* something went wrong when polling */
+			warnx("ubx: poll error");
 			return -1;
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			return handled ? 1 : -1;
+			if (handled) {
+				return 1;
+
+			} else {
+				warnx("ubx: timeout - no messages");
+				return -1;
+			}
 
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
@@ -292,8 +300,6 @@ UBX::receive(unsigned timeout)
 				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
 					if (parse_char(buf[i]) > 0) {
-						/* return to configure during configuration or to the gps driver during normal work
-						 * if a packet has arrived */
 						if (handle_message() > 0)
 							handled = true;
 					}
@@ -303,6 +309,7 @@ UBX::receive(unsigned timeout)
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
+			warnx("ubx: timeout - no useful messages");
 			return -1;
 		}
 	}
@@ -453,16 +460,16 @@ UBX::handle_message()
 					timeinfo.tm_min = packet->min;
 					timeinfo.tm_sec = packet->sec;
 					time_t epoch = mktime(&timeinfo);
-                
+
 #ifndef CONFIG_RTC
-                    //Since we lack a hardware RTC, set the system time clock based on GPS UTC
+					//Since we lack a hardware RTC, set the system time clock based on GPS UTC
-                    //TODO generalize this by moving into gps.cpp? 
+					//TODO generalize this by moving into gps.cpp?
-                    timespec ts;
+					timespec ts;
-                    ts.tv_sec = epoch;
+					ts.tv_sec = epoch;
-                    ts.tv_nsec = packet->time_nanoseconds;
+					ts.tv_nsec = packet->time_nanoseconds;
-                    clock_settime(CLOCK_REALTIME,&ts);
+					clock_settime(CLOCK_REALTIME, &ts);
 #endif
-                
+
 					_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
@@ -564,10 +571,13 @@ UBX::handle_message()
 
 		if (ret == 0) {
 			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
+			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
 
-			if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
+			hrt_abstime t = hrt_absolute_time();
+
+			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
 				/* don't attempt for every message to disable, some might not be disabled */
+				_disable_cmd_last = t;
 				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
 				configure_message_rate(_message_class, _message_id, 0);
 			}
@@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
 		ck_b = ck_b + ck_a;
 	}
 
-	/* The checksum is written to the last to bytes of a message */
+	/* the checksum is written to the last to bytes of a message */
 	message[length - 2] = ck_a;
 	message[length - 1] = ck_b;
 }
@@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
 
-	/* Calculate the checksum now */
+	/* calculate the checksum now */
 	add_checksum_to_message(packet, length);
 
 	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
 
-	/* Start with the two sync bytes */
+	/* start with the two sync bytes */
 	ret += write(fd, sync_bytes, sizeof(sync_bytes));
 	ret += write(fd, packet, length);
 
 	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: config write fail");
+		warnx("ubx: configuration write fail");
 }
 
 void
@@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
 	add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
 
-	// Configure receive check
+	/* configure ACK check */
 	_message_class_needed = msg_class;
 	_message_id_needed = msg_id;
 

src/drivers/gps/ubx.h

@@ -347,7 +347,7 @@ private:
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+	void			add_checksum_to_message(uint8_t *message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
@@ -358,7 +358,7 @@ private:
 
 	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
 
-	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
 
 	int			wait_for_ack(unsigned timeout);
 
@@ -376,7 +376,7 @@ private:
 	uint8_t			_message_class;
 	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_counter;
+	uint8_t			_disable_cmd_last;
 };
 
 #endif /* UBX_H_ */

src/drivers/px4io/px4io.cpp

@@ -2245,7 +2245,7 @@ px4io_main(int argc, char *argv[])
 		}
 
 		PX4IO_Uploader *up;
-		const char *fn[5];
+		const char *fn[3];
 
 		/* work out what we're uploading... */
 		if (argc > 2) {
@@ -2253,11 +2253,19 @@ px4io_main(int argc, char *argv[])
 			fn[1] = nullptr;
 
 		} else {
-			fn[0] = "/etc/extras/px4io-v2_default.bin";
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-			fn[1] = "/etc/extras/px4io-v1_default.bin";
+			fn[0] = "/etc/extras/px4io-v1_default.bin";
+			fn[1] =	"/fs/microsd/px4io1.bin";
 			fn[2] =	"/fs/microsd/px4io.bin";
-			fn[3] =	"/fs/microsd/px4io2.bin";
+			fn[3] =	nullptr;
-			fn[4] =	nullptr;
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+			fn[0] = "/etc/extras/px4io-v2_default.bin";
+			fn[1] =	"/fs/microsd/px4io2.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	nullptr;
+#else
+#error "unknown board"
+#endif
 		}
 
 		up = new PX4IO_Uploader;

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
+				     cm_uint16_from_m_float(gps.eph_m),
-				     gps.epv_m * 1e2f, // from m to cm
+				     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 */

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;
+
+	do {
+		/* do another attempt in case the unlink call is not synced yet */
+		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) {
-		/* 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);
-
 		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;