Merge branch 'gps_vel' of github.com:PX4/Firmware

src/drivers/gps/gps.cpp

@@ -285,6 +285,11 @@ GPS::task_main()
 		unlock();
 		if (_Helper->configure(_baudrate) == 0) {
 			unlock();
+
+			// GPS is obviously detected successfully, reset statistics
+			_Helper->reset_update_rates();
+			warnx("module configuration successful");
+
 			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
 //				lock();
 				/* opportunistic publishing - else invalid data would end up on the bus */
@@ -372,7 +377,11 @@ GPS::print_info()
 		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
 			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("update rate: %6.2f Hz", (double)_rate);
+		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);
+
+		_Helper->reset_update_rates();
 	}
 
 	usleep(100000);

src/drivers/gps/gps_helper.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -36,9 +36,32 @@
 #include <termios.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
 #include "gps_helper.h"
 
-/* @file gps_helper.cpp */
+/**
+ * @file gps_helper.cpp
+ */
+
+float
+GPS_Helper::get_position_update_rate()
+{
+	return _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
+
+float
+GPS_Helper::get_velocity_update_rate()
+{
+	return _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
+
+float
+GPS_Helper::reset_update_rates()
+{
+	_rate_count_vel = 0;
+	_rate_count_lat_lon = 0;
+	_interval_rate_start = hrt_absolute_time();
+}
 
 int
 GPS_Helper::set_baudrate(const int &fd, unsigned baud)

src/drivers/gps/gps_helper.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -33,7 +33,9 @@
  *
  ****************************************************************************/
 
-/* @file gps_helper.h */
+/** 
+ * @file gps_helper.h
+ */
 
 #ifndef GPS_HELPER_H
 #define GPS_HELPER_H
@@ -47,6 +49,15 @@ public:
 	virtual int			configure(unsigned &baud) = 0;
 	virtual int 			receive(unsigned timeout) = 0;
 	int 				set_baudrate(const int &fd, unsigned baud);
+	float				get_position_update_rate();
+	float				get_velocity_update_rate();
+	float				reset_update_rates();
+
+protected:
+	uint8_t _rate_count_lat_lon;
+	uint8_t _rate_count_vel;
+
+	uint64_t _interval_rate_start;
 };
 
 #endif /* GPS_HELPER_H */

src/drivers/gps/mtk.cpp

@@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
 	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
 
+	// Position and velocity update always at the same time
+	_rate_count_vel++;
+	_rate_count_lat_lon++;
+
 	return;
 }
 

src/drivers/gps/ubx.cpp

@@ -60,7 +60,8 @@
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 _fd(fd),
 _gps_position(gps_position),
-_waiting_for_ack(false)
+_waiting_for_ack(false),
+_disable_cmd_counter(0)
 {
 	decode_init();
 }
@@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
 		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
 		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
 		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
 		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
 		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
 
 		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
@@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
 		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
 
 		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
-		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
-
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_MSG;
-
-		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
-		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
-		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
-		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
-		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
-		/* For satelites info 1Hz is enough */
-		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
 
-		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
-
-		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-
-//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
-//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
+			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_TIMEUTC,
+			1);
+		// /* 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);
+		// /* 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);
+		// /* 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_DOP,
+		// 	0);
+		// /* 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_SVINFO,
+			0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
 
 		_waiting_for_ack = false;
 		return 0;
@@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
 	return -1;
 }
 
+int
+UBX::wait_for_ack(unsigned timeout)
+{
+	_waiting_for_ack = true;
+	int ret = receive(timeout);
+	_waiting_for_ack = false;
+	return ret;
+}
+
 int
 UBX::receive(unsigned timeout)
 {
@@ -498,6 +475,8 @@ UBX::handle_message()
 				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
 				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
 
+				_rate_count_lat_lon++;
+
 				/* Add timestamp to finish the report */
 				_gps_position->timestamp_position = hrt_absolute_time();
 				/* only return 1 when new position is available */
@@ -653,6 +632,8 @@ UBX::handle_message()
 				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
 				_gps_position->vel_ned_valid = true;
 				_gps_position->timestamp_velocity = hrt_absolute_time();
+
+				_rate_count_vel++;
 			}
 
 			break;
@@ -693,6 +674,12 @@ UBX::handle_message()
 
 		default: //we don't know the message
 			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			if (_disable_cmd_counter++ == 0) {
+				// Don't attempt for every message to disable, some might not be disabled */
+				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
+			}
+		return ret;
 			ret = -1;
 			break;
 		}
@@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
 	message[length-1] = ck_b;
 }
 
+void
+UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+{
+	for (unsigned i = 0; i < length; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+}
+
+void
+UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+{
+    struct ubx_cfg_msg_rate msg;
+    msg.msg_class	= msg_class;
+    msg.msg_id		= msg_id;
+    msg.rate		= rate;
+    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+}
+
 void
 UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
@@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned 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");
 }
+
+void
+UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+{
+	struct ubx_header header;
+	uint8_t ck_a=0, ck_b=0;
+	header.sync1 = UBX_SYNC1;
+	header.sync2 = UBX_SYNC2;
+	header.msg_class = msg_class;
+	header.msg_id    = msg_id;
+	header.length    = 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
+	_clsID_needed = msg_class;
+	_msgID_needed = msg_id;
+
+	write(_fd, (const char *)&header, sizeof(header));
+	write(_fd, (const char *)msg, size);
+	write(_fd, (const char *)&ck_a, 1);
+	write(_fd, (const char *)&ck_b, 1);
+}

src/drivers/gps/ubx.h

@@ -72,19 +72,20 @@
 #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
 
 #define UBX_CFG_RATE_LENGTH 6
-#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200		/**< 200ms for 5Hz */
 #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
 #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
 
 
 #define UBX_CFG_NAV5_LENGTH 36
-#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001		/**< only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
 #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
 #define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2			/**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
 
 #define UBX_CFG_MSG_LENGTH 8
 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
 
 #define UBX_MAX_PAYLOAD_LENGTH 500
 
@@ -92,6 +93,14 @@
 /** the structures of the binary packets */
 #pragma pack(push, 1)
 
+struct ubx_header {
+	uint8_t sync1;
+	uint8_t sync2;
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint16_t length;
+};
+
 typedef struct {
 	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
 	int32_t lon;					/**<  Longitude * 1e-7, deg */
@@ -274,11 +283,17 @@ typedef struct {
 	uint16_t length;
 	uint8_t msgClass_payload;
 	uint8_t msgID_payload;
-	uint8_t rate[6];
+	uint8_t rate;
 	uint8_t ck_a;
 	uint8_t ck_b;
 } type_gps_bin_cfg_msg_packet_t;
 
+struct ubx_cfg_msg_rate {
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint8_t rate;
+};
+
 
 // END the structures of the binary packets
 // ************
@@ -376,6 +391,14 @@ private:
 	 */
 	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
 
+	void			configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+
+	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);
+
+	int			wait_for_ack(unsigned timeout);
+
 	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
 	ubx_config_state_t	_config_state;
@@ -390,6 +413,7 @@ private:
 	ubx_message_class_t	_message_class;
 	ubx_message_id_t	_message_id;
 	unsigned		_payload_size;
+	uint8_t			_disable_cmd_counter;
 };
 
 #endif /* UBX_H_ */