Merge remote-tracking branch 'upstream/master'

2 files changed, 59 insertions, 31 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index f2e7ca67d..ad219cd25 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate)
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
@@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate)
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
-			0);
+			UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
@@ -224,35 +224,18 @@ UBX::receive(unsigned timeout)
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
-	uint8_t buf[32];
+	uint8_t buf[128];
 
 	/* timeout additional to poll */
 	uint64_t time_started = hrt_absolute_time();
 
-	int j = 0;
 	ssize_t count = 0;
 
-	while (true) {
-
-		/* pass received bytes to the packet decoder */
-		while (j < count) {
-			if (parse_char(buf[j]) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
-				 * if a packet has arrived */
-				 if (handle_message() > 0)
-					return 1;
-			}
-			/* in case we keep trying but only get crap from GPS */
-			if (time_started + timeout*1000 < hrt_absolute_time() ) {
-				return -1;
-			}
-			j++;
-		}
+	bool position_updated = false;
 
-		/* everything is read */
-		j = count = 0;
+	while (true) {
 
-		/* then poll for new data */
+		/* poll for new data */
 		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
 
 		if (ret < 0) {
@@ -272,8 +255,26 @@ UBX::receive(unsigned timeout)
 				 * available, we'll go back to poll() again...
 				 */
 				count = ::read(_fd, buf, sizeof(buf));
+						/* pass received bytes to the packet decoder */
+				for (int i = 0; i < count; i++) {
+					if (parse_char(buf[i])) {
+						/* return to configure during configuration or to the gps driver during normal work
+						 * if a packet has arrived */
+						if (handle_message())
+							position_updated = true;
+					}
+				}
 			}
 		}
+
+		/* return success after receiving a packet */
+		if (position_updated)
+			return 1;
+
+		/* abort after timeout if no packet parsed successfully */
+		if (time_started + timeout*1000 < hrt_absolute_time() ) {
+			return -1;
+		}
 	}
 }
 
@@ -327,6 +328,7 @@ UBX::parse_char(uint8_t b)
 			}
 			break;
 		case UBX_DECODE_GOT_CLASS:
+			{
 			add_byte_to_checksum(b);
 			switch (_message_class) {
 				case NAV:
@@ -413,6 +415,14 @@ UBX::parse_char(uint8_t b)
 //					config_needed = true;
 					break;
 				}
+				// Evaluate state machine - if the state changed,
+				// the state machine was reset via decode_init()
+				// and we want to tell the module to stop sending this message
+
+				// disable unknown message
+				//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
+				//configure_message_rate(_message_class, b, 0);
+			}
 				break;
 			case UBX_DECODE_GOT_MESSAGEID:
 				add_byte_to_checksum(b);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index df1958186..220842536 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -272,6 +272,11 @@ private:
 	 */
 	void _set_dlpf_filter(uint16_t frequency_hz);
 
+	/*
+	  set sample rate (approximate) - 1kHz to 5Hz
+	*/
+	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+
 };
 
 /**
@@ -378,7 +383,8 @@ MPU6000::init()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	_set_sample_rate(200); // default sample rate = 200Hz
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -494,6 +500,18 @@ MPU6000::probe()
 }
 
 /*
+  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
+*/
+void
+MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+{
+  uint8_t div = 1000 / desired_sample_rate_hz;
+  if(div>200) div=200;
+  if(div<1) div=1;
+  write_reg(MPUREG_SMPLRT_DIV, div-1);
+}
+
+/*
   set the DLPF filter frequency. This affects both accel and gyro.
  */
 void
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSAMPLERATE:
 	case ACCELIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSAMPLERATE:
 	case GYROIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS: